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ABSTRACT 

This  Software  Reference  documents  and  summarizes  all  source  code  produced  for  a 
Ph.D.  dissertation  constructing  an  underwater  virtual  world  for  an  Autonomous  Underwater 
Vehicle  (AUV). 

A  critical  bottleneck  exists  in  Autonomous  Underwater  Vehicle  (AUV)  design  and 
development.    It  is  tremendously  difficult  to  observe,  communicate  with  and  test  underwater 
robots,  because  they  operate  in  a  remote  and  hazardous  environment  where 
physical  dynamics  and  sensing  modalities  are  counterintuitive. 

An  underwater  virtual  world  can  comprehensively  model  all  salient  functional 
characteristics  of  the  real  world  in  real  time.    This  virtual  world  is  designed  from  the 
perspective  of  the  robot,  enabling  realistic  AUV  evaluation  and  testing  in  the  laboratory. 
Three-dimensional  real-time  computer  graphics  are  our  window  into  that  virtual  world. 

Visualization  of  robot  interactions  within  a  virtual  world  permits  sophisticated  analyses 
of  robot  performance  that  are  otherwise  unavailable.    Sonar  visualization  permits  researchers 
to  accurately  "look  over  the  robot's  shoulder"  or  even  "see  through  the  robot's  eyes"  to 
intuitively  understand  sensor-environment  interactions.    Extending  the  theoretical  derivation 
of  a  set  of  six-degree-of-freedom  hydrodynamics  equations  has  provided  a  fully  general 
physics-based  model  capable  of  producing  highly  non-linear  yet  experimentally-verifiable 
response  in  real  time. 

Distribution  of  underwater  virtual  world  components  enables  scalability  and  real-time 
response.    The  IEEE  Distributed  Interactive  Simulation  (DIS)  protocol  is  used  for  compatible 
live  interaction  with  other  virtual  worlds.    Network  connections  allow  remote  access, 
demonstrated  via  Multicast  Backbone  (MBone)  audio  and  video  collaboration  with 
researchers  at  remote  locations.    Integrating  the  World-Wide  Web  allows  rapid  access  to 
resources  distributed  across  the  Internet. 

This  dissertation  presents  the  frontier  of  3D  real-time  graphics  to  support  underwater 
robotics,  scientific  ocean  exploration,  sonar  visualization  and  worldwide  collaboration. 
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I.    INTRODUCTION 

The  purpose  of  this  guide  is  to  document  the  source  code  developed  in  the 
creation  of  an  underwater  virtual  world  for  an  autonomous  underwater  vehicle  (AUV). 
it  is  companion  document  to  a  dissertation  (Brutzman  94).    There  are  a  number  of 
parts  to  this  work.    First  a  users  guide  is  presented  which  describes  how  to  obtain  and 
install  the  software  de.scribed  here.    Key  mission  output  files  then  show  results 
produced  by  the  autonomous  underwater  robot  interacting  with  the  virtual  world. 
Remaining  chapters  pre.sent  the  source  code  developed  for  robot  execution  software, 
3D  computer  graphics,  six  degree-of-freedom  hydrodynamics,  a  geometric  sonar 
model,  networking,  and  hypermedia  using  the  World-Wide  Web  and  the  Multicast 
Backbone  (MBone). 

Several  languages  are  included  in  this  source  code.    Robot  execution  programs 
and  supporting  configuration  files  are  written  in  the  Kemigan  and  Richie  variant  of  the 
C  language,  and  run  identically  under  Irix  5.2  or  OS-9  operating  systems.    Other 
source  programs  are  written  primarily  in  C-n-.    Source  code  file  name  conventions  use 
a  .(  extension  for  C  language  files,  and  a  .C  extension  for  C++  language  files. 
Remaining  programs  and  files  are  written  in  languages  specific  to  the  application,  such 
as  iinuplot  (Williams  95),  Tool  control  language  {Tel)  (Osterhout  94)  (Welch  94), 
hypertext  markup  language  (.html)  (Berners-Lee  94a,  94b)  (Hughes  94)  or  operating 
system  shell  scripts. 

All  of  the  programs  described  here  are  available  electronically  without  charge  to 
Internet  users  via  anonymous  ftp.    The  accompanying  public  electronic  distribution  of 
this  reference  includes  both  source  code  and  compiled  executable  programs.    All 
programs  have  been  tested  under  SGI  operating  system  Irix  5.2,  and  robot  code  has 
also  been  tested  under  operating  system  OS-9  version  2.3.    Goals  of  the  underwater 
virtual  world  project  include  making  source  code,  applications  and  interfaces  easily 
available  in  order  to  encourage  scientific  collaboration  and  educational  use.    Future 


work  includes  porting  virtual  world  viewers  to  a  variety  of  operating  systems  and 
computer  architectures. 

An  NPS  AUV  hypermedia  home  page  has  been  prepared  to  facilitate 
examination  and  download  of  the  various  underwater  virtual  world  components.    This 
home  page  is  available  for  interaction  using  any  number  of  Internet-based  browsers, 
most  notably  Mosaic  (NCSA  94a).    Resources  on  the  home  page  include  the  electronic 
distribution,  installation  and  execution  instructions,  pointers  to  related  work,  images, 
plots,  papers  and  a  variety  of  other  media.    World-Wide  Web  (WWW)  Uniform 
Resource  Locator  (URL)  for  the  NPS  AUV  home  page  is: 
frp:/ /ramus. cs. nps.navy.mil/pub/auv/auv. html 

To  learn  more  about  the  World-Wide  Web,  consult  "Entering  the  World-Wide 
Web  (WWW):  A  Guide  to  Cyberspace"  (Hughes  94).   This  paper  describes  everything 
needed  to  start  using  Mosaic,  accessing  all  types  of  information  sources,  and  using 
hypermedia  and  the  Internet.   Mosaic  is  a  hypermedia  browser  that  is  freely  available 
for  Unix  machines  running  X  windows.  Macintoshes  and  personal  computers  (PCs) 
running  Windows.    There  are  also  free  line  mode  browsers  (such  as  lynx  and  www) 
thai  let  you  browse  hypertext  documents  in  a  simple  text  mode  using  any  computer 
which  has  an  internet  connection  (Montulli  94)  (Berners-Lee  94b). 

The  source  code  in  this  software  guide  is  fully  documented  and  reasonably 
self-explanatory.   Graphics  and  hydrodynamics  programs  are  written  in  the  ANSI 
standard  C++  language,  robot  execution  code  and  networking  code  is  written  in  the 
Kernighan  and  Richie  variant  of  the  C  language,  plotting  programs  use 
gnuplot  version  3.5  scripting  language,  and  hypertext  pages  are  written  in  standard 
Hypertext  Markup  Language  {.html)  (NCSA  94b).  Despite  extensive  documentation, 
however,  one  programmer's  clearly-written  code  may  well  be  undecipherable  to 
another  programmer.    Users  are  strongly  encouraged  to  review  the  companion 
dissertation  which  derives  the  formulae  and  explains  the  concepts  behind  the  software 
(Brutzman  94),  retrieve  the  latest  version  of  the  underwater  virtual  world  distribution, 
or  contact  the  author  for  questions  that  remain  unanswered.   No  guarantees  of  any 


kind  are  implied.    Many  problems  are  open  and  progress  occurs  on  a  frequent  basis 
when  conducting  research  on  robots  and  virtual  worlds. 

Hopefully  this  work  will  stimulate  and  encourage  other  research  efforts  to  build 
large-scale  virtual  worlds  in  compatibly-extendable  ways.    Interested  readers  are 
encouraged  to  examine  the  AUV  home  page  for  project  updates,  read  the 
documentation,  and  to  contact  the  author  regarding  new  uses  of  the  software  and 
potential  research  collaboration. 


II.    INSTALLATION,  EXECUTION,  CREATION  AND  REMOVAL 

A.       Introduction 

The  following  Installation  and  Execution  Guide  is  intended  to  enable  any  new 
user  to  download,  install,  execute  and  understand  the  NPS  AUV  Underwater  Virtual 
World.    Users  may  want  to  solely  download  the  viewer  software  to  observe  an 
Internet-wide  mission  exercise,  or  run  the  entire  set  of  programs  making  up  the  virtual 
world.    Users  of  the  robot  execution  program  can  enter  their  electronic  mail  address 
and  receive  a  mission  report  following  each  mission.    A  sample  mission  report  shows 
the  mission  script  used  for  the  canonical  SIGGRAPH  94  mission,  a  project  abstract, 
pointers  to  related  work  and  the  execution  orders  generated  by  the  mission  script. 
Mission  reports  are  also  available  by  "fingering"  the  home  auv  account,  e.g. 

fitificr  auy(a  dudccs. nps.navy.mil 

Finally,  shell  script  files  for  archive  creation  and  removal  are  included  to 
document  automated  archive  maintenance. 


B.      auv-uvw.  INSTALL  Installation  and  Execution  Guide 

//  - - - // 

Installation  &  Execution  Guide  for  the  NPS  AUV  Underwater  Virtual  World 
auv-uvw. INSTALL  19  OCT  94 

Don  Brutzman  brut zmananps .navy .mil 

URL:  ftp : //taurus . cs .nps .navy .mil/pub/auv/auv-uvw .virtual-world. INSTALL 

// - --- -  // 

Table  of  Contents 

Distribution  information 

System  requirements 

Retrieving  the  distribution  auv-uvw .tar . Z 

Installing  the  distribution  auv-uvw . tar . Z 

MBone  connection  (optional  but  recommended) 

Information  files 

Viewing  remotely-executing  robot  missions  via  the  MBone 

Running  all  of  the  virtual  world  components  locally 

Plotting  mission  telemetry  using  gnuplot 

Recompiling  virtual  world  component  executables 

Killing  multicast  processes 

Removing  the  entire  distribution 

Future  work 

Contact  information 

This  is  a  big  project!   I've  tried  to  make  this  guide  as  concise  and  readable 
as  possible.   However  some  work  is  necessary  to  read  and  absorb  this  material. 

Suggestions  on  how  to  further  streamline  the  guide  are  very  welcome.   Thanks! 

// - -- // 

Distribution  information 

For  the  latest  greatest  distribution,  plus  pointers  to  all  of  the  supporting 
public  domain  programs,  use  Mosaic  or  a  World-Wide  Web  line  browser  to 
connect  to  the  NPS  AUV  home  page: 

ftp : //taurus . cs . nps . navy . mil/pub/auv/auv . html 
To  retrieve  the  distribution  directly: 

ftp : //taurus . cs .nps .navy.mil/pub/auv/auv-uvw. tar .Z 
To  receive  a  copy  of  a  recent  mission  report: 
finger  auvadude.c8.nps.navy.mil 

// - // 

System  requirements 

The  auv-uvw  'viewer'  requires  SGI  Irix  5.2  or  better  with  Openlnventor  2.0 
Execution  Only  Environment  (inventor_eoe)  installed.   This  is  provided 
free  with  all  versions  of  Irix,  it  merely  needs  to  be  installed. 
You  can  tell  if  it  is  installed  by  typing  'versions'  at  the  SGI  prompt. 

Modification  and  recompilation  of  the  'viewer'  requires  installation  of 


inventor_dev  Inventor  3D  Toolkit,  2.0  development  option. 
The  virtual  world  'viewer'  is  written  in  C++  using  Openlnventor 
graphics,  and  it  also  uses  the  NPSNET  DIS  2.0.3  multicast  libraries 
(which  are  also  included  as  source/binaries  in  this  distribution) . 

The  'dynamics'  virtual  world  software  has  been  tested  under  Irix  5.2, 
is  written  in  C++,  and  also  uses  the  NPSNET  DIS  2.0.3  multicast 
libraries.   You  should  run  it  on  a  sound-equipped  workstation  for 
best  results. 

The  robot  'execution'  code  has  been  tested  under  Irix  5.2  and  OS -9  v2 . 3 
It  is  written  in  ANSI  C  with  special  ttdefines  for  OS-9  KStR  C 
compatibility,  allowing  it  to  be  run  without  modifications  either  under 
Irix  or  on  the  NPS  Autonomous  Underwater  Vehicle  68030  microprocessor. 
It  ought  to  be  portable  to  most  any  plain  vanilla  C  compiler. 

// - // 

Retrieving  the  distribution  auv-uvw. tar . Z 

The  easiest  way  is  to  retrieve  &  save  the  tar  file  is  to  point  Mosaic  at 

ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

If  instead  you  use  anonymous  ftp,  here  is  a  sample  session: 

unix>>  ftp  taurus.cs.nps.navy.mil 
Connected  to  taurus.cs.nps.navy.mil. 

220  taurus  FTP  server  (Version  6.10  Wed  Mar  18  11:57:03  PST  1992)  ready. 

Name  (taurus . cs .nps .navy .mil :yourname) :  anonymous 
331  Guest  login  ok,  send  e-mail  address  as  password. 
Password:  your .email . address .here 

ftp>  cd  pub/auv 

250  CWD  command  successful. 

ftp>  binary 

200  Type  set  to  I. 

ftp>  get  auv-uvw . tar . Z 

local:  auv-uvw . tar . Z  remote:  auv-uvw . tar . Z 

150  Opening  BINARY  mode  data  connection  for  auv-uvw. tar . Z (4747669  bytes) 

226  Transfer  complete . 

4747669  bytes  received  in  34.32  seconds  (135.08  Kbytes/s) 

ftp>  quit 

221  Goodbye . 

The  uncompressed  distribution  is  about  10  MB,  not  counting  the  extra 
recommended  applications.   The  compressed  distribution  is  about  5  MB. 
Precise  distribution  sizes  can  vary  when  changes  are  incorporated. 

// - - - // 

Installing  the  distribution  auv-uvw. tar .Z 

Uncompress  auv-uvw .tar . Z  in  your  root  directory  by  typing 
Unix : yourhome >     uncompress  auv-uvw. tar . Z 
unix:yourhome>     tar  -xvf   auv-uvw. tar 

Backup  k   update  your  MBone  session  directory  conf icfuration  file  .sd.tcl 
Unix : yourhome >     cp   .sd.tcl  .sd.tcl. old 


Unix : yourhome >     cp   .sd. tcl .auv-uvw   .sd.tcl 

Edit  the  MBone  tool  aliases  to  include  directory  paths  as  appropriate. 
They  are  located  at  the  very  end  of  the  new  file  .sd.tcl 

Unix : yourhome >     zip  .sd.tcl 

After  editing,  exit  and  restart  session  directory  (sd) : 
Unix : yourhome  >     s  d 

You  can  now  run  the  viewer  from  an  sd  advertisment  for  the  NPS  AUV  Underwater 
Virtual  World  (if  there  is  one  running  -  this  is  optional) . 

You  will  notice  some  auv-uvw  information  files  in  your  root  directory 
and  several  new  directories : 

yourhome/auv-uvw/  Openlnventor  viewer  program  &  data  files 

yourhome/execution/AUV  (robot)  execution  level  code  and 
mission  scripts 

yourhome/dynamics/Virtual  world  for  robot:  dynamics,  sonar 
and  other  components 

yourhome/dynamics/speeclText-to-speech  audio  files  from  mission 
scripts.   These  originate  from  queries 
to  the  Say.,  server 

The  auv-uvw  distribution  should  now  be  fully  installed. 

// - - --// 

MBone  connection  (optional  but  recommended) 

Multicast  Backbone  (MBone)  connectivity  is  needed  if  you  want 
to  participate  in  remote  exercises.   A  good  way  to  test  your  networ)c's 
connectivity  is  to  type  'sd'  (session  directory) .   If  you  get  a  menu 
of  MBone  programs  starting  to  slowly  build  up,  you  are  in  business. 

To  receive  Distributed  Interactive  Simulation  (DIS)  robot  execution 
Protocol  Data  Units  (PDUs)  originating  on  your  own  networ)c,  you 
do  not  have  to  be  connected  to  the  Internet-wide  MBone. 

SGI's  Irix  5.2  operating  system  supports  multicast  in  the  kernel,  so 
you  can  still  run  the  robot/virtual  world  programs  on  one  machine  & 
the  auv-uvw  viewer  on  another.   Only  one  process  per  machine  can  grab 
the  multicast  port,  thus  at  least  2  machines  are  needed  to  r\in 
everything . 

To  connect  your  system  to  the  MBone  takes  some  work  but  is  worth  it. 
You  will  need  the  involvement  6  support  of  your  network  administrator. 

The  following  article  tells  why  MBone  is  so  great  and  how  to  connect: 

ftp://tauru8.C8.nps.navy.mil/pub/mbmg/mbone.ps  (PostScript)  or 
ftp://tauru8.c8.np8.navy.mil/pub/mbmg/mbone.html  (hypertext)  or 
ftp://tauru8.C8.np8.navy.mil/pub/mbmg/mbone.txt    (ASCII  text) 

Other  installation  pointers  are  on  the  NPS  MBone  home  page  at 


ftp : //taurus . cs .nps .navy.mil/pub/mosaic/mbone .html 

Over  a  thousand  subnets  worldwide  are  already  connected  to  the  MBone . 
There  are  lots  of  people  willing  to  help.   In  practice  we've  seen  it 
take  people  between  a  few  days  and  a  few  weeks  to  start  using  MBone, 
but  once  connected  further  work  is  rarely  required.   Good  luck! 

// - - -- - // 

Information  files: 

auv-uvw. virtual -world. README    (a  mission  report) 
auv-uvw. virtual -world. INSTALL   (this  file) 

others  are  listed  in  the  mission  report  and  the  home  page  site 

// // 

Viewing  remotely-executing  robot  missions  via  the  MBone 

You  must  have  MBone  connectivity  installed  on  your  network  for  remote 
experiment  observation.   If  the  NPS  AUV  Underwater  Virtual  World 
is  advertised  in  sd,  and  you  have  installed  the  distribution, 
just  select  the  session  and  multicast  ports  are  passed  automatically 
to  the  viewer. 

Alternatively,  you  can  start  the  viewer  manually  from  the  command  line: 

unix:yourhome>     auv-uvw .viewer 
or    unixtyourhome/auv-uvwak/'iewer 

The  following  command  line  switches  are  available  with  viewer: 

-address  224 . 2 . ###  .  ##nulticast  address  as  advertised  in  sd  session 
-port     #####     multicast  wb  port  as  advertised  in  sd  session 

The  virtual  world  viewer  should  now  run. 

// - // 

Running  all  of  the  virtual  world  components  locally 

Running  a  robot  mission  takes  three  processes  and  at  least  two  machines, 
viewer  and  dynamics  must  be  on  different  hosts,  execution  can  run  anywhere. 

Notes  regarding  sd  session  advertisements  are  optional. 

*  First  is  the  'viewer'  to  get  a  window  into  the  virtual  world.   Run  this 
on  your  most  capable  Iris  workstation  -  Reality  Engines  are  nice! 
You  can  use  or  disable  texturing  based  on  the  capabilities  of  the  machine. 

unixl>   cd  auv-uvw 
unixl>  viewer 

The  following  optional  command  line  switches  are  available  with  viewer: 

-address  224  .  2  .  ###  .  ##nulticast  address  as  advertised  in  sd  session 
-port  #####       multicast  wb  port  as  advertised  in  sd  session 

-texture-on 

-texture-off 

-printdialog    pop  up  print  dialog  box  for  screen  snapshots 

-noprintdialog 

The  virtual  world  viewer  now  runs . 
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•  The  'dynamics'  program  is  the  virtual  world  connection  for  robot  execution: 
Run  it  on  a  sound-equipped  workstation  for  best  results,  otherwise  ignore  any 
sound-card-missing  error  messages.   The  'dynamics'  program  is  purely 

text -based  so  graphics  capabilities  are  irrelevant. 

unix2>   cd  dynamics 
\inix2>    dynamics 

Dynamics  classes  test  selections 

L  loop_test_with_execution_level  ( ) ; 

M  Multicast  parameter  input 

ttl=15,  group  address=224 . 2 . 121 . 93 ,  port=3111 

0  Ocean  current  vector  reset   <0,  0,  0> 
H  Hmatrix/quatemion  exerciser 

R  Rotation  of  quaternion  &  Hmatrix  using  p  q  r 
D  Defaults 

1  Invert  matrix  test 

E  dEad_reckon_test_with_execution_level 
P  PDU_8kip_interval  change  (from  1) 
T  Toggle  TRACE  =  0 
C  DIS_net_close  ( ) ; 
Q  Quit 
Enter  choice : 

Select  L  for  loop  test  with  execution  level 

The  following  command  line  switches  are  available  with  dynamics  program: 

-ttl  #       time  to  live  (be  careful!)  recommend  16  or  less 

-address  224 . 2 . ###  .  #^ulticast  address  as  advertised  in  sd  session 

-port  #####    multicast  wb  port  as  advertised  in  sd  session 
-loop  start  looping  automatically 

♦  Finally,  'execution'  is  the  robot  execution  level  software, 
'execution'  runs  through  the  file  'mission . script '  and  gets 
real  world  responses  from  the  virtual  world  program  'dynamics' . 
'execution'  can  run  under  Unix  or  real-time  operating  system  OS-9. 

unix3>    cd  execution 

unix3>    cp  mission . script . the_one_you_want  mission . script 

unix3>    execution   remotehost  unixl .host .name 

Enter  your  e-mail  in  the  execution  process  window  when  asked. 

See  the  file  mission . script .HELP  for  info  on  how  to  write  mission  scripts. 

//-- - // 

Plotting  mission  telemetry  using  gnuplot 

unix3 :yourhome>    cd  execution 

unix3  :yourhome/execution/:gnuplot   auv_plot.gnu 

For  fewer  points  plotted  (1  per  second)  out  of  the  same  dataset : 

unix3  ryourhome/execution/jgnuplot   auv_plot_l_second.gnu 

or  use  a  postscript  viewer  (xpsview  or  ghostview)  to  look  at  plots  directly: 

unix3>  xpsview    -*ifp  -skipc  -or  landscape  -/execution/AUV_telemetry .ps  & 
or  unix3>  ghostview  -landscape  -/execution/AUV_telemetry .ps  t 

// - - ---// 


Recompiling  virtual  world  component  executables 

auv-uvw  directory 

(Requires  inventor_dev  Inventor  2.0  development  option  installed. 
See  system  requirements  section  for  details) . 

unix3 :yourhome/auv-uvw/>    make  viewer 

dynamics  directory 

unix3 :yourhome/ dynamics />   make  dynamics 

execution  directory 

unix3 :yourhome/execution/>   copy  Makefile. SGI  Makefile 
unix3 :yourhome/execution/>  make  execution 

or   OS-9:  yourhome/execution/>   copy  Makefile. 0S9  Makefile 
OS-9:  yourhome/execution/>  make  execution 

Details  on  recompilation  and  modification  are  included  in  all  source  files. 
// .// 

Killing  multicast  processes 

Sometimes  multicast  processes  (such  as  'viewer'  and  'dynamics')  do  not  die 
cleanly  because  they  have  forked  a  second  multicast  process . 
viewer  is  especially  prone  to  this  behavior  when  invoked  from  session 
directory  (sd) ,  since  sd  must  put  the  viewer  process  into  the  background. 

You  may  need  to  take  special  action  to  ensure  the  processes  are  killed. 
The  following  instructions  will  work  on  SGI  machines: 

To  put  a  process  to  sleep  that  refuses  to  let  go  of  the  command  prompt: 

type  a  "Z  (control-Z)  in  the  runaway  process  window 
To  see  what  processes  are  running: 

unix>  ps 
To  see  ALL  processes  that  are  running  (including  previous  zombie  processes) : 

unix>  ps  -ea 
To  kill  a  process: 

unix>  kill  -9  ####  where  ####  is  the  Process  ID  (PID) 

An  example  log: 

[runaway  process,  you  hit  the  control-Z  key  to  suspend  it] 

"Z 

Suspended 

unix>>  ps 

PID  TTY  TIMK  COMD 

16558  ttyq3  9:37  viewer 

11289  ttyq3  0:01  tcsh 

16581  ttyq3  0:00  viewer 

16902  ttyq3  0:00  ps 
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unix>>  kill  -9  16558  16581 

unix>>  ps 

PID  TTY  TIME  COMD 

11289  ttyq3  0:01  tcsh 

16904  ttyq3  0:00  ps 

// // 

Removing  the  entire  distribution: 

Be  sure  to  move  any  files  that  you  want  to  save  to  different  directories. 

You  do  not  have  to  remove  the  old  distribution  prior  to  updating 
to  a  new  version.   However  you  might  want  to,  so  that  superceded  files 
in  old  distributions  are  not  left  over. 

To  remove  everything,  rxin  the  shell  script  auv-uvw .virtual-world. REMOVE: 

unix>  source    auv-uvw .virtual -world. REMOVE 

//  - - // 

Future  work,  (lots!) 

Porting  the    Openlnventor  viewer  code  to  other  architectures  and 
porting  other  virtual  world  components  to  other  architectures 
depends  on  availability  of  Openlnventor  and  multicast  TCP/IP  support. 
Porting  is  planned  for  1995. 

Other  people  are  always  welcome  to  use  and  extend  this  code. 
Please  keep  me  inforroed  of  your  efforts  if  you  find  it  of  value. 

Other  research  collaborations  to  extend  the  underwater  virtual  world 
to  build  up  a  truly  large-scale  Internet-wide  fully  distributed 
virtual  world  are  of  particular  interest. 

Integrate  Dr.  Larry  Ziomek's  Recursive  Ray  Acoustics  (RRA)  sonar  model. 

Incorporate  pre-processed  terrain  datasets  for  Monterey  Bay. 

Virtual  Reality  Modeling  Language  (VRML)  -  see  the  working  group 
http : //www . wired . com/vrml/ 

Dissertation  "A  Virtual  World  for  an  Autonomous  Underwater  Vehicle" 
is  being  written  and  will  be  made  publicly  available. 

// // 

Contact  information: 

Don  Brutzman  brut zman»nps .navy .mil 

Code  OR/Br    Naval  Postgraduate  School   [Glasgow  204]   work  (408)  656-2149 
Monterey  California  93943-5000   USA  fax   (408)  656-2595 

// - // 
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C.      mission,  ou^ut.  email  Mission  Output  Electronic  Mail  Report 

The  following  report  is  sent  by  electronic  mail  to  users  who  execute  a  robot 
mission.   It  demonstrates  that  robots,  once  networked,  can  report  back  results  only 
when  they  are  of  interest  or  human  intervention  is  specifically  desired.   The  goal  is 
for  the  robot  to  meet  user  directives  conveniently,  rather  than  requiring  constant 
monitoring  or  supervision. 

The  mission  report  combines  an  NPS  AUV  reference  information  file,  the 
mission  script  that  drove  the  robot,  and  the  resulting  time  line  of  propulsor  and  plane 
surface  orders.    It  is  also  available  by  querying  the  robot  account  with  the  Unix 
"finger"  command:  finger  auv@cs.dude.nps.navy.mil 

NPS  AUV  Mission  Report 

Here  is  a  mission  report  generated  by  the  Naval  Postgraduate  School  (NPS) 
Autonomous  Underwater  Vehicle  (AUV)  running  in  an  Underwater  Virtual  World. 

This  message  was  created  by  the  operating  robot  connected  to  the  network. 
Actual  robots  with  network  connections  can  send  reports  such  as  this 
to  research  teams  and  interested  individuals  when  scientific  discoveries 
are  made  or  unusual  circumstances  occur. 


virtually  yours,  the  NPS  AUV.  .         ....  8-/  nps  auv  ) 

.   ...  8-\ ) 


The  following  Universal  Resource  Locators  (URL's)  are  pointers  that  lead  to 
more  information  about  the  NPS  AUV  Underwater  Virtual  World  &  related  work. 

*  The  AUV  underwater  virtual  world  source  code,  information  files  and 

*  executable  programs  (for  SGI  Irix  5.2  machines)  can  be  used  to  monitor 

*  NPS  AUV  DIS  PDU's  from  anywhere  on  the  Internet  MBone .   It  is  freely 

*  available  via  anonymous  ftp  as  a  compressed  tar  file  at 
ftp : //taurus . cs .nps .navy.mil/pub/auv/auv_uvw .tar. Z 

*  NPS  AUV  home  page  free  software  distribution,  research  summary  and 

*  anonymous  ftp  directory: 

ftp: //taurus .cs .nps .navy.mil/pub/mosaic/auv.html 
ftp: //taurus . cs .nps .navy.mil/pub/auv/AUVPAPERS 
ftp: //taurus . cs .nps .navy.mil/pub/auv/ 

*  Naval  Postgraduate  School  World-Wide -Web  (WWW)  home  page  includes  many 

*  additional  pointers  to  the  NPSNET  Virtual  Battlefield,  DIS,  MBone,  and 

*  I3LA  regional  research  around  Monterey  Bay: 

ftp: //taurus .cs .nps .navy.mil/pub/mo8aic/nps_mosaic.html 

*  To  learn  about  Internet  audio  &  video  on  the  Multicast  Backbone  (MBONE) : 
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ftp ://taurus . cs .nps .navy.mil/pub/mosaic/mbone .html 

♦  How  the  robot  voice  was  synthesized  live  from  text  over  the  Internet  using 

♦  Axel  Belinfante's  "Say..."  speech  server  at  University  of  Twente, 

*  Netherlands  which  uses  Nick  Ing-Simmons'  phoneme  synthesizer  'rsynth': 
http : //utisl79 . cs.utwente.nl : SOOl/say/? 

♦  Postscript  plots  (20  pages)  of  vehicle  telemetry  during  SIGQRAPH  mission: 
ftp: //taurus . cs .nps .navy.mil/pub/auv/SIGGRAPH94/AUV_telemetry.ps .Z 

*  The  people  from  NPS  involved  in  SIGGRAPH  94  _The  Bdge_  NPS  AUV  exhibit: 
ftp: //taurus . cs .nps .navy.mil/pub/pratts/home .html 

•  Learning  more  about  the  World-Wide -Web,  URLs  and  Mosaic: 

♦  "Entering  the  World-Wide  Web:  A  Guide  to  Cyberspace"   by  Kevin  Hughes 
http  :  //vrvfw  .  eit .  com/web/www  .  guide/  hypertext 

ftp: //ftp .eit . com/pub/web. guide/guide . 61/guide . 61 .ps .Z   Postscript 
ftp://ftp.eit . com/pub/web. guide /guide . 61/guide . 61 .txt    text 

*  A  recent  copy  of  this  mission  report  can  be  found  by  an  auv  account  finger: 
finger  auv®dude . cs .nps .navy .mil 

If  you  can't  find  taurus.c8.nps.navy.mil,  it  has  IP  number  131.120.1.13 
Additional  references  are  available  on  request. 

This  project  is  part  of  a  PhD  dissertation.   Here  is  the  abstract: 

A  Virtual  World  for  an  Autonomous  Underwater  Vehicle 

Don  Brutzman 

Code  OR/Br,  Naval  Postgraduate  School 

Monterey  California  93943-5000  USA 

(408)  656-2149  office,   (408)  656-2595  fax 

brutzman®nps .navy.mil 

A  critical  bottleneck  exists  in  Autonomous  Underwater  Vehicle  (AUV) 
design  and  development.   It  is  tremendously  difficult  to  observe,  communicate 
with  and  test  underwater  robots,  because  they  operate  in  a  remote  and 
hazardous  environment  where  physical  dynamics  and  sensing  modalities  are 
counterintuitive . 

An  underwater  virtual  world  can  comprehensively  model  all  salient 
functional  characteristics  of  the  real  world  in  real  time.   This  virtual 
world  is  designed  from  the  perspective  of  the  robot,  enabling  realistic  AUV 
evaluation  and  testing  in  the  laboratory.   3D  real-time  graphics  are  our 
window  into  that  virtual  world.   Visualization  of  robot  interactions  within 
a  virtual  world  permits  sophisticated  analyses  of  robot  performance  that 
are  otherwise  unavailable.   Sonar  visualization  permits  researchers  to 
accurately  "look  over  the  robot's  shoulder"  or  even  "see  through  the 
robot's  eyes"  to  intuitively  understand  sensor-environment  interactions. 

Distribution  of  underwater  virtual  world  components  enables  scalability 
and  real-time  response.   The  IEEE  Distributed  Interactive  Simulation  (DIS) 
protocol  is  used  for  compatible  live  interaction  with  other  virtual  worlds. 
Network  access  allows  individuals  remote  access.   This   is  demonstrated  via 
MBONE  collaboration  with  others  outside  The  Edge,  and  Mosaic  access  to 
pertinent  archived  images,  papers,  datasets,  software,  sound  clips,  text 
and  any  other  computer-storable  media. 

This  project  presents  the  frontier  of  3D  real-time  graphics  for 
underwater  robotics,  ocean  eaqiloration,  sonar  visualization  and  worldwide 
scientific  collaboration. 


Questions,  comments  and  collaborations  are  welcome.   Please  contact: 
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Don  Brutzman  brutzmananps.navy.mil 

Code  OR/Br    Naval  Postgraduate  School   [Glasgow  204]   work  (408)  656-2149 
Monterey  California  93943-5000   USA  fax   (408)  656-2595 

SIGGRAPH  94  Collaborators:   Michael  J.  Zyda ,  Paul  T.  Barham,  John  S.  Falby, 

Anthony  J.  Healey,  Shirley  Isakari,  Rodney  Luck, 
Michael  R.  Macedonia,  Robert  B.  Mcghee, 
David  R.  Pratt,  Lawrence  J.  Ziomek 
Naval  Postgraduate  School,  Monterey  California 


Your  AUV  robot  mission  completed  successfully.   Here  is  the  mission  you  ran: 

#  your  mission  is 

#  mission. script .rotate_test 
time  0 
timestep  0 . 1 

#  initialize  vehicle 
depth  45 

position     0    0   45 

orientation  0    0    0 

thrusters-on 

rotate   16 

pause 

wait  60 

rotate  0 

thrusters-of f 

wait  120 

step 

keyboard 

keyboard- of f 

#  mission  complete 
kill 

#  NPS  AUV  file  mission .output .orders :  commanded  propulsion  orders  versus  time 

#  timestep:   0.10  seconds 
# 

#  time   heading  North  East  Depth    rpm   rpm    stern   stern   vertical     lateral 

#  X    y     z      port   stbd   plane   rudder   thrusters   thrusters 

#  bow/stern   bow/stern 

0.0      0.0    0.0    0.0    0.0     0.0     0.0     0.0     0.0     0.0    0.0    0.0    0.0 

#  timestep:   0.10  seconds 

0.0      0.0    0.0    0.0   45. 0 

60. 0      0.0    0.0    0.0   45. 0 

180 .1      0.0    0.0    0.0   45. 0 


0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0 
0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0 
0.0     0.0     0.0     0.0     0.0    0.0    0.0    0.0 
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D.     make jauvuvw Jar  Archive  Creation  Script 

#  make_auv_uvw_tar :   NPS  AUV  Underwater  Virtual  World  archive  creation  script 

#  Don  Brutzman    2  0  OCT  94 

echo  create  auv  underwater  virtual  world  distribution  tar  file  auv-uvw. tar . Z 

#  Notes: 

#  do  not  make  this  tar  on  gravyl ,  it  doesn't  have  enough  memory  for  'speech' 

#  ensure  mission. script . siggraph  was  the  latest  (best)  mission  run 

#  this  script  is  not  included  in  the  distribution. 

#  NPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

#  ftp://taurus .CB .nps .navy.mil/pub/mosaic/auv.html 

cd   /n/dude /work/brut zman 

echo     'removing  old  files...' 
rm   -f    auv-uvw. tar 
rm   -f    auv-uvw . tar . Z 

#  Standard  e-mail  report  becomes  the  README  file 

rm   -f  auv-uvw. virtual -world. README 

cp        execution/mission .output .email  auv-uvw .virtual -world. README 

chmod  -w  auv-uvw .virtual -world .README 

tar  -cvf  auv-uvw. tar  auv-uvw .virtual -world. README 

rep       auv-uvw .virtual-world. README  taurus : -f tp/pub/auv 

chmod  +w  auv-uvw .virtual -world. README 

rm   -f  auv-uvw. virtual -world. README 


execution/auv-uvw . INSTALL 


rm   -1 

cp 

chmod  -w 

rep       auv-uvw .virtual -world. INSTALL 

tar  -rvf  auv-uvw. tar 

chmod  +w 

rm   -f 

rm   -f 

cp        execution/auv-uvw. REMOVE 

chmod  +x 

tar  -rvf  auv-uvw. tar 

rm 


auv-uvw .virtual-world . INSTALL 
auv-uvw . virtual -world . INSTALL 
auv-uvw .virtual -world. INSTALL 
taurus : -f tp/pub/auv 
auv-uvw. virtual -world. INSTALL 
auv-uvw .virtual-world . INSTALL 
auv-uvw .virtual-world. INSTALL 

auv-uvw. virtual -world. REMOVE 
auv-uvw . virtual -world . REMOVE 
auv-uvw .virtual -world. REMOVE 
auv-uvw .virtual -world . REMOVE 
auv-uvw. virtual -world. REMOVE 


rep  execution/mission . script . HELPtaurus : -f tp/pub/auv 

tar  -rvf  auv-uvw. tar  auv-uvw .viewer 

tar  -rvf  auv-uvw. tar  auv-uvw/viewer 

tar  -rvf  auv-uvw. tar  auv-uvw/viewer .C 

tar  -rvf  auv-uvw. tar  auv-uvw/Makef ile 

tar  -rvf  auv-uvw. tar  auv-uvw/overhang .rgb 

tar  -rvf  auv-uvw. tar  auv-uvw/auv. iv 

tar  -rvf  auv-uvw. tar  auv-uvw/ Jason . iv 

tar  -rvf  auv-uvw. tar  auv-uvw/Platf orm. iv 

tar  -rvf  auv-uvw. tar  auv-uvw/Testtank. iv 

tar  -rvf  auv-uvw. tar  auv-uvw/testtank.C 

tar  -rvf  auv-uvw. tar  auv. html 

tar  -rvf  auv-uvw. tar  mbone.html 

tar  -rvf  auv-uvw. tar  nps_mosaic.html 

tar  -rvf  auv-uvw. tar  .sd.tcl .auv-uvw 

tar  -rvf  auv-uvw. tar  .mailcap .auv-uvw 
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tar  -rvf  auv-uvw.tar  . cshrc- excerpt .auv-uvw 

tar  -rvf  auv-uvw.tar  execution/execution 

tar  -rvf  auv-uvw.tar  execution/execution . c 

tar  -rvf  auv-uvw.tar  execution/parse_f unctions . c 

tar  -rvf  auv-uvw.tar  execution/globals . c 

tar  -rvf  auv-uvw.tar  execution/globals .h 

tar  -rvf  auv-uvw.tar  execution/def ines .h 

tar    -rvf   auv-uvw.tar   execution/ermo.h 

tar  -rvf  auv-uvw.tar  e xecut ion /modes .h 

tar  -rvf  auv-uvw.tar  execution/setsys .h 

tar  -rvf  auv-uvw.tar  execution/sgstat .h 

tar  -rvf  auv-uvw.tar  execution/Makefile* 

chmod  -w  execution/mission. script .HELP 

tar  -rvf  auv-uvw.tar  execution/mission.* 

chmod  644  execution/mission. script .HELP 

tar  -rvf  auv-uvw.tar  execution/auv_plot .gnu 

tar  -rvf  auv-uvw.tar  execution/auv_plot_l_second.gnu 

tar  -rvf  auv-uvw.tar  execution/printplots 

tar  -rvf  auv-uvw.tar  execution/disbridge . c 

tar  -rvf  auv-uvw.tar  execution/os9sender . c 

tar  -rvf  auv-uvw.tar  execution/os9server . c 

tar  -rvf  auv-uvw.tar  dynamics /dynamics 
tar  -rvf  auv-uvw.tar  dynamics/Makefile 
tar  -rvf  auv-uvw.tar  dynamics/*. H 
tar  -rvf  auv-uvw.tar  dynamics/*. C 

echo  'remove  speech/numbers .au  originating  from  high  replication  counts...' 

dynamics/speech/2? .au 

dynamics/speech/3? .au 

dynamics/speech/4? . au 

dynamics/speech/5? .au 

dynamics/speech/6? . au 

dynamics/speech/7? .au 

dynamics/speech/8? .au 

dynamics/speech/9? .au 

dynamics/speech/1?? .au 

dynamics/speech/2?? .au 

dynamics/speech/3?? .au 

dynamics/speech/4?? .au 
echo  'the  speech  directory  can  get  very  large  and  occasionally  needs  pruning, 
tar  -rvf  auv-uvw.tar  dynamics/speech/* .au 

tar  -rvf  auv-uvw.tar  DIS.mcast/* 

#  The  following  files  are  provided  separately  to  reduce  tar  file  size. 

#  See  the  auv  home  page  for  remote  access  instructions 

#  ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

#  tar  -rvf  auv-uvw.tar  execution/AUV_telemetry .ps 

#  tar  -rvf  auv-uvw.tar  aaai92ws .ps .Z 

#  tar  -rvf  auv-uvw.tar  dynamics /www 

#  tar  -rvf  auv-uvw.tar  execution/gnuplot 

Is  -1     auv-uvw.tar 

echo  'compressing  tar  file...' 

compress   auv-uvw.tar 

Is  -1     auv-uvw. tar . Z 

echo  'rep  auv-uvw . tar . Z  taurus : -ftp/pub/auv' 
rep  auv-uvw. tar .Z  taurus : -ftp/pub/auv 

echo  "archive  information  on  taurus  anonymous  ftp  server:" 

rsh  taurus  'Is  -1  -f tp/pub/auv/auv-uvw. tar . Z' 
echo  'make_auv_uvw_tar  complete.' 
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rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

rm 

-f 

E.      auv-uvw.virtual-world. REMOVE  Archive  Removal  Script 

#  auv-uvw.virtual-world. REMOVE   archive  removal  shell  script 

#  To  run  this  script,  please  type  the  following  at  the  unix  pron^t : 

#  chmod  +x  auv-uvw.virtual-world. REMOVE 

#  source    auv-uvw. virtual -world. REMOVE 

#  Don  Brutzman   19  OCT  94 

#  NPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

#  ftp : //taurus . cs . nps . navy . mil/pub/mosaic/auv . html 

echo  'remove  auv  underwater  virtual  world  distribution...' 
echo  'be  sure  you  run  this  from  the  directory  you  installed  it' 
echo  ' (ordinarily  your  home  directory) ' 

rm   -f        auv-uvw. tar 
rm   -f        auv-uvw .tar . Z 

echo  'warning:   unaliasing  rm...' 
unalias  rm 

rm  -f  -r  execution/* 

rm  -f  -r  dynamics/* 

rm  -f  -r  auv-uvw/* 

rm  -f  -r  DIS.mcast/* 

#  remove  any  inadvertantly  created  .dot  files 
rm      -f  -r  execution/.* 

rm      -f  -r  dynamics/.* 
rm      -f  -r  auv-uvw/.* 
rm      -f  -r  DIS.mcast/.* 

rmdir  execution 

rmdir  dynamics 

rmdir  auv-uvw 

rmdir  DIS.mcast 

rm  -f  auv-uvw .viewer 

rm  -f  auv. html 

rm  -f  mbone.html 

rm  -f  nps_mosaic.html 

rm  -f  . sd. tcl . auv-uvw 

rm  -f  .mailcap .auv-uvw 

rm  -f  . cshrc-excerpt .auv-uvw 

rm  -f  auv-uvw. virtual -world. README 

rm  -f  auv-uvw .virtual -world. INSTALL 

rm  -f  auv-uvw. virtual -world. REMOVE 

rm  -f  auv-uvw.* 

echo  'restoring  .sd.tcl  with  your  saved  .sd. tcl. old,  please  confirm. 

echo  cp  .sd.tcl. old  .sd.tcl 
cp  .sd.tcl. old  .sd.tcl 

echo  'auv-uvw. virtual-world. REMOVE  complete.' 
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m.   NPS  AUTONOMOUS  UNDERWATER  VEHICLE  EXECUTION  LEVEL 

A.      Introduction 

The  execution  level  of  robot  software  is  the  hard  real-time  process  which 
controls  propellers,  thrusters,  plane  surfaces,  sonars  and  other  sensors.    Hardware 
interfaces  to  the  robot  microprocessor  are  controlled  by  the  execution  level.   The 
execution  level  is  further  tasked  with  interprocess  communications  widi  the  tactical 
and  strategic  levels  of  the  robot  architecture  (Byrnes  93),  as  well  as  the  virtual  world 
when  operating  in  the  laboratory. 

The  execution  code  presented  here  has  been  tested  under  a  wide  variety  of 
laboratory  conditions.   Regrettably  much  of  the  interface  code  to  actual  vehicle 
hardware  has  not  been  tested  since  early  1994  when  a  battery  explosion  caused  major 
damage  to  the  NPS  AUV  II.   Hardware  repairs  from  February  through  October  1994 
were  required  before  in-water  testing  resumed.   Nevertheless  pre-explosion  hardware 
interface  source  code  has  been  retained  in  place  and  clearly  identified  through 
comments  and  compile  flags.    Future  work  includes  verifying  hardware  interface  code 
(Marco  95),  updating  the  execution  code  and  then  running  the  combined  virtual 
world/real  world  version  of  the  execution  level  in  the  water. 

An  important  benefit  of  a  robot  connected  to  an  integrated  simulator 
(Brutzman  92a,  92b,  92c)  or  underwater  virtual  world  (Bnitzman  94)  is  that  robot 
software  development  can  continue  independently  of  actual  robot  readiness. 
Approximately  half  of  the  execution  level  source  code  presented  here  was  developed 
during  the  eight  month  period  that  the  NPS  AUV  II  was  out  of  commission.   This 
advantage  alone  proved  to  be  strong  justification  for  the  value  of  providing  a  network 
connection  to  the  actual  robot  microprocessor  and  software.  The  essential 
requirement  for  integrated  simulation  in  an  underwater  robotics  research  and 
development  program  has  been  subsequently  confmned  by  other  researchers 
(Trimble  94)  (Kuroda  94). 
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Another  important  strength  of  the  robot  software  is  that  the  Kernigan  and  Richie 
variant  C  language  source  code  is  able  to  compile  correctly  under  two  dissimilar 
operating  systems:   SGI  Irix  5.2,  a  Unix  variant,  and  OS-9  version  2.3,  a  real-time 
operating  system  that  includes  features  similar  to  both  Unix  and  DOS.   Compiler 
directives,  customized  makefiles  and  command  line  aliases  are  used  to  provide  this 
cross-platform  portability.   Network  communication  via  sockets  has  also  been  made 
compatible  between  these  two  operating  systems.   This  flexibility  has  been  invaluable 
for  nq)id  and  effective  software  development.   One  example  out  of  many  is  that 
diagnosis  and  debugging  tools  under  Unix  are  far  more  robust  than  under  OS-9, 
permitting  detection  and  correction  of  subtle  difficulties  not  detectable  by  the 
OS-9  C  language  compiler  and  linker. 

Future  work  includes  upgrading  execution  and  tactical  level  communication 
links.   Currently  communications  between  levels  use  serial  and  parallel  ports,  a  highly 
customized  and  error-prone  hardware  arrangement.   By  adding  Ethernet  connectivity 
to  the  tactical  level  computer,  all  software  levels  will  be  able  to  communicate  via 
sockets  regardless  of  what  networked  processor  or  workstation  they  might  physically 
reside  on.    Additional  long  term  goals  include  extending  Internet  protocol 
compatibility  to  acoustic  telemetry  communications.    Ultimately  underwater  robots 
will  be  independent  nodes  on  the  Internet  regardless  of  whether  they  are  in  the 
laboratory,  operating  with  a  tether  or  swimming  freely  and  autonomously  within  an 
acoustic  local  area  network  (ALAN).   The  sophistication  of  virtual  worlds  will  grow 
closer  to  that  of  the  real  world  as  robots  and  researchers  operate  in  each. 
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B.      mission,  script,  HELP  Robot  Execution  Level  Mission  Script  Syntax 


//• 


-/execution/mission. script .HELP  11  October  94 

Mission  script  syntax  for  NPS  AUV  execution  level  control  in 

the  NPS  AUV  Underwater   Virtual  World 
Don  Brutzman  brut ztnan»nps  .navy  .mil 


//--- - 

This  file  describes  how  to  change  and  create  NPS  AUV  mission  script  files. 
Files  and  the  'execution'  program  are  in  the  -/execution  subdirectory. 

To  run  a  new  mission,  copy  an  existing  mission  file  over  file 
' mission. script '  or  edit  the  mission. script  file  for  a  new  mission. 


Example : 


unix>  cd  execution 

xinix>  cp   mission  .  script .  siggraph   mission  .  script 

unix>  execution   remotehost  fletch.cs.nps.navy.mil 


Script  commands  are  read  by  AUV  execution  level  (execution . c) 

from  the  "mission .script"  default  file  at  the  start  of  each  mission. 

Some  of  the  following  commands  will  also  work  when  invoked  from  the  command 
line  upon  execution. 

Here  are  script  keywords  (and  synonyms)  that  are  currently  recognized: 


■// 


■// 


Keywords 
Synonyms 


Parameters  j 
[optional]  | 


Description 
(units  in  feet  or  degrees  as  appropriate) 


HELP 

/? 


Provide  a  list  of  available  keywords 
(as  specified  in  this  HELP  file)  . 


REMOTEHOST   hostname 
REMOTE      hostname 


// 
/* 
# 

WAIT 

execute) 

RUN  # 

TIME  # 

WAITUNTIL        # 
PAUSEUNTIL      # 

TIMESTEP         # 
TIME-STEP        # 

PAUSE 


tells  execution  level  to  open  socket  to  virtual  world 
which  is  already  executed  and  waiting  on  'hostname' 
REMOTEHOST  is  a  command  line  switch.   Example: 
unix>  execution   remotehost  fletch.cs.nps.navy.mil 

comments  follow  on  this  line  which  are  not  executed 

note  comments  will  still  be  spoken  if  AUDIO-ON 

pound  sign  also  indicates  a  comment  if  in  first  column 

#       Wait  (or  run)  for  #  seconds  (letting  the  robot 

prior  to  reading  from  the  script  file  again 

Wait  (or  rvm)  until  robot  clock  time  # 
(letting  the  robot  execute  its  current  orders) 
prior  to  reading  from  the  script  file  again 

change  default  execution  level  time  step  interval 
from  default  of  0.1  sec  to  #  sec 

temporarily  stop  execution  until  <enter>  is  pressed 
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REALTIME 
REAL-TIME 

NOREALTIME 

N.:  -f.ea:_':":me 

NuNREA^TIKE 
NOWAIT 
NO- WAIT 
NOPAUSE 
NO- PAUSE 

MISSION  filename 
SCRIPT  filename 
FILE    filename 

hxEYBoAP  I 

keyboar:-on 

keyboare-off 
no- keyboard 


run  execution  level  code  in  real-time 

(busy  wait  at  the  end  of  each  timestep  if  time  remains) 

run  execution  level  code  as  quickly  as  possible 


Wv  i  . 

ET^'iT 

DCN'E 

EXIT 

REPEAT 

RESTART 

COMPl^ETE 

<ec.f>  marker 

KILL 

Snl'TL'^WN 

R  ?K 

* 

i*#l 

SPEEL 

* 

l*«i 

PROPS 

« 

I#«] 

PPOPELLDRS 

M 

I  ««  j 

COURSE 

« 

HEADING 

YAW 

« 

TURN 

« 

CHANGE- COURSE 

« 

Replace  'mission. script '  with  'filename'  and  start 
the  new  mission 


read  script  commands  fromi  keyboard 


read  script  commands  from  mission . script  file 


do  not  execute  any  more  commands  in  this  script,  but 
repeat  the  mission  again  if  LOOP-FOREVER  is  set 


RUDDER  « 

DEADSTICKRUDDER  [«: 

DEPTH  * 

PLANES  « 

DEADSTICK PLANES  !«; 

THRUSTERS-ON 
THRUSTERS 
THRUSTERON 
THRUSTERSON 

NOTHRUSTER 
NOTHRUSTERS 
THPUSTERS-OFF 
THFUSTERSOFF 

ROTATE  # 


same  as  QUIT  but  also  shuts  down  socket  to  virtual  world 
'dynamics'  process. 

Set  ordered  rpir  values  to  #  for  both  propellers 
[  or  independently  set  left  &  right  rpm  values 

to  M   and  *#  respectively] 
maximum,  propellor  speed  is  +-  700  rpm  =>  2  ft/sec 

Set  new  ordered  course  (commanded  yaw  angle) 


Change  ordered  course  by  #  degrees 

(positive  *  to  starboard,  negative  #  to  port; 

Force  rudder  to  remain  at  #  degrees 

Force  rudder  to  remain  at  0  [or  «]  degrees 

Set  new  ordered  depth  (commanded  z) 

Force  planes  to  remain  at  #  degrees 

Force  planes  to  remain  at  0  [or  #]  degrees 

Enable  vertical  and  lateral  thruster  control 


Disable  vertical  and  lateral  thruster  control 


open  loop  lateral  thruster  rotation  control 

at  «  degrees/sec 
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NOrOTATE 
ROTATEOFF 
ROTATE- OFF 

LATERAL 


NOLATERAL 
LATERALOFF 
i^A  .  nr.A.^  -  -Jf  F 

POSITION 
LOCATION 


ORIENTATION   «  «4t  ##« 
ROTATION      «  #*  #*# 


CONTINUE 

GO 

STEP 

SINGLE-ST 

EP 

TRACE 

TFA'T-E-ON 

TPJ-.CEGFF 

TrACE-OFF 

NCTFJt-.CE 

NO- TRACE 

LOOPFOREV 

ER 

LOOP- FOP E 

VER 

XP'jNCE 


disable  open  loop  lateral  thruster  rotation  control 


#       open  loop  lateral  thruster  translation  control 

at  *t  ft/ sec 

(positive  is  to  starboard,  maximum  is  0.5  ft/sec) 

disable  open  loop  lateral  thruster  translation  control 


reset  vehicle  dead  reckon  position  to 
(X,  y,  z)  =  {#,  ##,  «##) 

reset  vehicle  orientation  to 
(phi,  theta,  psi)  =  (#,  *#,  ###) 

continue  reading  script  &  executing,  no  action  performed 


loop  for  another  timestep  prior  to  reading  script  again. 
Particularly  useful  in  keyboard  mode. 

enable  verbose  print  statements  in  execution  level 


disable  verbose  print  statements  in  execution  level 


repeat  current  mission  when  done. 

each  repetition  is  called  a  'replication.' 

do  not  LOCFFOREVER,  stop  when  end  of  script  is  reached 


L-  1  Pr  :L:^cACK'.'P 
LOOP- FILE-BACKUP 


EI-:TEP:-ONTPOLCON£TAr.'T£ 
ENTEP -CONTROL-CONSTANTS 

SLIDINGMODECOURSE 
SLI LING-MODE-COURSE 

SLIDINGMOLEOFF 
SLIDING-MODE-OFF 

SONARTRACE 

SONARTRACEOFF 

SONAR  INSTALLED 

PARALLELPORTTRACE 

AUDIBLE 
AUDIO 
AUDIO- ON 
SOUND- ON 
SOUNDON 
SOUND 


back  up  output  files  during  each  loop  replication 
to  permit  inspection  while  new  files  are  written 
the  backup  files  are  in  execution  directory: 
output . telemetry . previous  &  output . l_second .previous 

start  a  keyboard  dialog  to  enter 
revised  control  algorithm  coefficients 

Sliding  mode  course  control  algorithm  (not  yet  working) 


Disable  sliding  mode  course  control  algorithm 

enable  verbose  print  statements  in  execution  sonar  code 
disable  verbose  print  statements  in  execution  sonar  code 
sonar  interface  hardware  cards  are  installed,  use  them 
enable  trace  statements  for  parallel  port  communications 
enable   text-to-speech  audio  output 
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EZLEN'T  disable  text-to-speech  audio  output 

liZZO'CKL 
SCUNDOFF 
SOUND -OFF 
ALT' I  OOF  F 
AUDIO-OFF 
QUIET 

EMAIL  ask  user  for  electronic  mail  address  at  mission  start, 

EM;-.IL-0N  send  user  an  electronic  mail  report  at  mission  finish 

E - MA I L 

E-MAIL- ON 
EVJ-.  I LON 

E]'-1A.:„  .'F F  disable  electronic  mail  address  query  feature 

EKJ;iL-OFF 

E-M^i:^CFF 

E-M^IL-OFF 

NC-E-MiAIL 

NC- EMAIL 

NO-E-M-.IL 

NCEMiA.IL 

WAYPOINT    ♦*>:  #Y  [«2]  Point  towards  waypoint  with  coordinates  («X,  «Y) 
WAV'CI.'.'T- CN  «X  «Y  l«iz;   (depth  *Z  optional).   Leave  waypoint  control  by 

ordering  course,  rudder,  sliding-mode,  rotate  or 

lateral  thruster  control. 

WAYPGINTFGLLOW         Set  mode  to  arrive  at  each  waypoint  before  reading  the 
WAYFOI'.'T-FCLLOW         next  mission  script  command,  i.e.  continue  towards  each 

waypoint  for  however  long  it  takes  to  reach  the  standoff 
distance  before  pausing  to  read  the  next  command. 

WAYPCINTFOLLOWOFF       Disables  WAYPOINTFOLLOW  mode 
WAYPOINT- FCLLOW-OFF 

£TA;::ofF  *    change  standoff  distance  for  WAYPOINT- FOLLOW  and  HOVER 

STANZ-OFF  #    control 

STAN'LCFFLISTANCE  « 

STANDOFF-  r  I  ST.AIvCE  * 

£'TA.'JI  -  ^FF - Z  I STA'-iC'E  * 

.H:A"Eh       1*X  ^♦Y  1«Z]]  Hover  using  thrusters  and  propellers  for  longitudinal 

and  lateral  positioning  at  specified  or  previous 
waypoint 

H-.Er       *A  «Y   »Z   [  «orientat..oni   [  #standof  f -distance] 

Uses  WAYPOINT  control  until  within  #standof f-distance 
of  HOVER  point  («X,  #Y,  «Z) ,  then  switches  to 
HOVER  control  with  [optional]  final  #orientation 

Full  speed  (700  RPM)  port  &  starboard  is  used  if 

AUV  distance  to  WAYPOINT  is  >  #standof f-distance  +  10', 

then  slows  to  200  RPM  until  within  #standof f-distance, 
then  HOVER  control. 
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C.  execution.c  Robot  Execution  Level  Real-Time  Control  Program 


■*****«***i 


«»«♦»»«»*»«**»•♦**«•**«•»»♦»»»' 


PiociiaiT;:         execution. c  AU\'  execution  level  program 

Author?:         Don  Brutzman,  Dave  Marco  &  Walt  Landaker 

Revised:         2  8  October  94 

S-ystem:  AW  Gespac  68020/68030,  OS-9  version  2.4 

Compiler:        Gespac  cc   Kernighan  &  Richie  (K&R)  C 

Comipilation :     ftp>     put  execution.c 
auvsiml>  chd  execution 
[68020]       auvsiml>  make  -k2f  execution 
[68030]       auvsiml>  make      execution 

[Irix  i      fietch>  make  execution 

E>:<=-cur  ..on : 

[Irix  J       fletch>  cd  execution 

fletch>    execution   remote  dynamics-hostname 

where  dynamics-hostname  is  the  IP  name  of  the  host  running 
the  dynamics  (virtual  world;  program. 

Plotting:        see  gnuplot  scripts  ' auv_plot .gnu '  and  'auv_plot_l_second .gnu ' 
fletcn..-  gnuplot   auv_plot.gnu 

Iiebuaqir.  J  :       gravyl  : -brutzman 'execution>>  lint  -Im  execution.c 

lint  -Im  -Iglobals.h  -Idefines.h  globals.c  parse_functions . c  \ 

execution . c 

fletch>  make  warnings 


iption:     closed  loop  for  operation  during  vehicle  in-water 

rriissions  as  well  as  in  virtual  world 


Active  changes:   Don  Brutzman    working  lab/virtual  world  networked  version 

&  tactical  level  interface 

Dave  Marco      working  vehicle  code 

L   interfacing  physical  devices 

Future  work:     Update  digital  <==>  analog  access  for  new  vehicle  hardware 

Retest  code  after  vehicle  repaired 

Sonar/altimeter  integration  code  reintegrated/retested 

Audio  strings  seem  to  be  generated  differently  by  OS-9 

standardize  parsing  of  command  line  and  script  coiranands 

finish  sliding  mode  control 

change  serial /parallel  comms  to  sockets  once 
tactical  level  gets  an  Ethernet  card 

Testing  interprocessor  connections: 
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parallel  pore 


serial  port 


Interfaces 


/P  OS-9   auvsirr.l>  mfi_a3 

LPTI :       DOS   auvsim2>  portfix 

>  print  f ilename . txt 

(/Tl)  /TT   OS-9  auvsiml>  wr2cl  then  write  text 

OS-9  auvsiinl>  rdtla  then  read  text 

DOS   auvsiin2>  C:\COMM\PROCOMM 

then  <alto>  for  chat  mode 
<altF10>  help,  <altX>  exit 


Telemetry  sent 
Telemetry  received 


via  serial   port  /tt  [==  /tl  at  high  baud  rate] 
via  parallel  port  /P 


Telenieti-y  j?  optionally  passed  to/from  tactical  level  running  on  80386 


Read.-  t:les: 
Wr-tes  files: 


mission . init  [mission  initialization  data  file  ; 
output. data  [vehicle  telemetry  state  vector  data; 
output. auv    [tactical  order/executive  report  log; 


Sr-nar  commands/repl  ies  via  device  pert  /t3 

Note  that  %F  double  formats  are  used  instead  of  %lf  on  scanf ( )  and  sscanf ( ) 
calls  for  OS-9  compatibility 


♦•include  "globals.h" 
♦include  "defines. h" 


luncticn  prototypes 


r*»«»«««*««««»«***«»*»«**««- 


■*«**««»***•*«: 


:***«VV***«**«  «*** 


tl'jere  sciie  way  to  put  parameter  specifications  in  the  prototypes??      */ 
or.ly  1  f  we  buy  the  ANSI  C  compiler  from  Microware  (or  shift  to  VxWorks)  •/ 


void 

closed_loop_control_modul 

e 

void 

get_control_constants 

ic  -;.1t 

depth 

dodi:-a.e 

calculate_psi 

void 

zerc_gyrc_data 

vc^  j 

initialize_dac£ 

VG^d 

initialize_adcs 

void 

contrcl_surface 

VC  -  ^ 

rudder 

vcid 

planes 

void 

main_motors_of f 

void 

alive 

vcid 

record_data_on 

void 

record_data_of f 

void 

record_data 

douDle 

roll_angle 

double 

pitch_angle 

double 

heading 

double 

roll_rate_gyro 

double 

pitch_rate_gyro 

double 

yaw_rate_gyro 

double 

s t bd_mo t o r_ rpm 

double 

p o r t_mo t o r_rpm 

double 

get_speed 

void 

get_init_avg 

void 

get_avg_rng 

void 


open_device_paths 


(); 
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vc;d 

close_device_paths 

vcid 

read_pa  ra 1 1 el_por c 

vc:d 

daci 

void 

dac2b 

:rit 

add 

■   ,-  f 

adc2 

void 

Init_PortA 

void 

Init_PorcB 

uri^igi 

,ed 

char 

Read_PortA 

unsigned 

char 

Read_PortB 

unsigned 

short 

Read_PortAB 

void 

set_bsyA 

void 

rst_bsyA 

int 

ck_sta 

void 

CGnt:er_sonar 

char 

query_sonar_l_repl 

void 

set:_scep_si2e 

vcid 

tty  mode 

vc :  Q 


voia 


print_valid_keyword£ 

oper._vartual_world_socke!: 
shutdown_virtuaI_world_socket 
3end_dat:a_on_virrual_worId_socket 
get;_srream_f  roiT._virtual_world_socket 


acuD^e 

Degrees 

dcutle 

r adi ans 

double 

ncrrT.ali  ze 

OC -L.e 

;■.  oriTia  1  i  Zfe2 

dOuLle 

radian_nonnali2e 

ioUDle 

r  a  d  i  a  n_no  rma  i i z  e2 

void 

ciamp 

d0UDl<r 

a  I.  a  r.  ^ 

double 

Sinn 

double 

cosh 

dour,  le 

can  |-i 

aouc-r  sign 

/'    iroiTi  pa r 3e_f unct ions  . c 


extern  void  parse_command_line_f lags 

extern  void  parse_mission_script_comiriands 

extern   void  parse_rTiission_string_cominands 


0 


r  «  *  It  *  »  «  1 


r**ik*it«**'***ir**«****' 


/'  0£-9  ~  specific  function  compatibility 

*. f  def incd  ( sgi ! 

void    tsleep    (unsigned  svalue)  (  /•  null  body  */} 

void    _sysdate  (format,  time,  date,  day,  tick) 

int  format,  *time,  *date,  *tick;  short  *day; 
I  /■•  null  body  •/) 

double  pow      (xx,  yy)      f   power  function  */ 
double  XX,  yy ; 
{return  exp  (yy  *  log  (xx) ) ; ) 

int     _gs_rdy   (path)  int  path;  {  return  0;  )  /•  bytes  waiting  on  path  •/ 

inc     _gs_opt   (path,  buffer) 
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int  path;  struct  sgbuf  *buffer; 
;  return  0;  ) 

ir.t     _?s:_opt   (path,  buffer) 

int  path;  struct  sgbuf  *buffer; 
i    return  0;  } 

/***«**■»*»**«**«********»****«**************♦*♦**♦♦**♦*♦•*************»»♦******/ 

/«««»■»■»•»*****»»♦»♦****»»**»*«*****»♦♦«■**«**«»*********»«*************»**«******/ 

ri.:.  'iijc,  argv 

:nt  aigc;  char  **argv;     /*  command  line  arguments  */ 
i 

if  (TRACE  iir  DISPLAYSCREEN)  printf  ("[start  main:   execution]  \n" )  ; 

.-trcpy  (v:r:ja:_wcrld_remote_host_name,  VIRTUAL_WORLD_REMOTE_HOST_NAME)  ; 

d:  =  TIMECTEP; 

parse_comrriand_line_f  lags    (argc,    arg\';  ; 

ge:_ccrtrci_constants  (); 

in:::ali  ze_dacs  ( >  ; 
init_aiize_adcs  ( ) ; 

oper._de V i  c e_P'a  t  n s  (  )  ; 

:eccrd_data_Dr.  (;;  /*  Open  files  for  data  logging       •/ 

ope!._virtual_wcrId_socket  {);         /•  open  connection  to  virtual  world  •/ 

if  ;strlen  (buffer)  >  0) 

;^-^.-id_data_on_virtUal_world_socket  {);    /•  SILENT?  send  to  sound  driver  */ 

strcpy  (buffer,  "  A  U  V  virtual  world  socket  is  open"); 

send_data_or._v:  rtual_world_sockec  ();  /■*  buffer  containing  message  sent  */ 

do     /'  wr.^lt-  (LOCATIONLAE  Uk    LDCPFOREVER )  •/ 

;     / *  indefinite  repeat  loop  for  long-duration  lab  testing  */ 

. */ 

if  .:.IS?LAY£CREEN) 
1 

if  (LOCATIONLAB  &&  (EMAIL  ==  TRUE)) 
i 

Strcpy  (buffer,  "  Please  Enter  Your  E-mail  Address'); 

send_data_on_virtual_world_socket  ();  /*  buffer  containing  msg  sent  */ 

printf  (•%s  •**  HERE  *••:   ",  buffer); 

strcpy  (email_address,  *"); 

gets  (email_address)  ; 

sprintf  (buffer,  'Thanks  %s\n',  email_address) ; 

printf  ('%s\n',  buffer); 

send_data_on_virtual_world_socket  ();  /*  buffer  containing  msg  sent  */ 

if  ((int)  (strlen  (email_address)  >  2)  && 

(strcmp  {email_address,  'brutzman')  !=  0)  && 
(strcmp  (email_address,  'BRUTZMAN")  !=  0)) 
{ 

emailaddressfile  =  fopen  (E34A I LADDRESS FILENAME,  'a');   /•  append  */ 
fprintf  (emailaddressfile,  "%s\n",  email_address) ; 
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fclose   (emailaddressf ile) ; 
) 

els-e    if   LOCATIONLAB  ==  FALSE) 

alive  ilO,  starc_dwel 1 i ;  /*  Wag  fin  every  10  seconds  for  total  duration 

of  start_dwell  seconds  */ 

printf("   Position  AUV  for  Directional  Gyro  Offset  Measurement \n *) ; 
printf ("   Rate  Gyro  zero  measurementXn' ) ; 

printfC   Hit  <Enter>  on  AUV  When  Ready   •*•  Here  !  ***\n'); 
answer  =  getchar  ( )  ,-  /*  pause  */ 
) 

printf  ("   OK!!   Starting  the  mission . \n" ) ; 
} 
parse_mission_script_commands  ( ) ;     /*  read  initial  script  orders      */ 

2erc_gyrc_data  ( ) ;  /*  Get  daily  zeros  for  gyros       •/ 

if  (SONARIKSTALLED)  center_sonar  (  )  ;   /■•  must  have  open_device_paths  1st  */ 

5L:cpy  (ij-ffer,  "  ,,,,");  /*  pause  */ 

serjd_data_on_virtual_world_socket  ( )  ;   /*  buffer  containing  msg  sent  •/ 
L^t.'cpy  'Jjjffe:",  "  A  U  V  is  starting"); 
send_data_on_virtual_world_socket  ( ) ;   /*  buffer  containing  msg  sent  */ 

'  In: : i 5l : zat ion  of  closed  loop  parameters  •/ 

bjf  f  er_inde>:  =    0 

tcrl  femeti"^'_records_saved   =    0, 
n"ii3Sion_leg_counter  =    0, 

end_test  =    FALSE; 

wrap_coL;nt  =    0; 

=    0.0; 

./ 

/  ■"    Main  program  operational  loop  code  */ 

it  (TRACE  ii,  DISPLAYSCREEN) 

printf  ("[Starting  miain  program  operational  loop  code...  ]*); 

previousloopclock  =  clock  (); 

/. ./ 

wnile  (end_test  ==  FALSE)  /*  this  is  the  realtime  main  operational  loop  */ 

/*  when  end_test  ==  TRUE  then  loop  is  done    •/ 
{ 

closed_loop_control_module  ();  /*  closed  loop  code  is  here  <««««<*/ 

I                          /*  end  of  real-time  main  operational  loop  */ 
/. ♦/ 

/ *  lab  version  may  repeat  forever  for  long-duration  testing:  */ 

replication_count  ++; 

if  (LOCATIONLAB  &&  LOOPFOREVER  &&  DISPLAYSCREEN) 
( 

printf  CVnlLOOP  FOREVER  enabled,  next  loop  is  replication  %d...)\n", 
replication_count ) ; 

sprintf  (buffer,  ■  LOOP  FOREVER  enabled,  next  loop  is  replication'); 

send_data_on_virtual_world_socket  ();     /*  buffer  msg  sent  */ 

sprintf  (buffer,  '  %d',  replication_count ) ; 

send_data_on_virtual_world_socket  ();     /*  buffer  msg  sent  */ 
) 

if  (LOCATIONLAB  &&  LOOPFOREVER) 
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/'    reset    aonounc   of    time   to  wait    for   next    conur.and   */ 

t  inie_next_coiniriand   =    0.0; 

c  =0.0; 

if  (D1£PLAYSCREEN; 

{ 

prir.tf  ( '  \nLoopf  orever  reset  time:   [  time_next_command  =  0.0]  "); 

print f  ( "   [t  =  0 .0]  \n" ) ; 


i:  (LOOPFILEBACK'JP! 

record_data_of f  (); 

*.t   def  .r,frd  (sgi  ; 

piintf  ••m.  output  .  telemetry  .previousXn' 

system  ("rm  output .  telemetry' .previous* 

printf  ('cp  mission . output . telemetry  output . telemetry .previousXn" 

system  ("cp  mission .output . telemetry  output . telemetry .previous" 

printf  ("rm  output . l_second.previous\n" 

system  ("rm  output . l_second. previous* 

printf  ('cp  mission .output . l_second  output . l_second.previous\n* 

system,  ^"cp  mission . output . l_second  output . l_second. previous* 


#e:3t 


«^r.j.  I 


printf  ("del  output . telemetry  .previous\n' 

syste.T  ("del  output .  telemetry  .previous* 

printf  (*copy  mission. output . telemetry  output . telemetry .previousXn' 

syster:  '"copy  mission  .  output .  telemetry  output .  telemetry  .previous* 

printf  ("del  output . l_second . previous\n " 

systeiT  ("del  output  .  l_second.  previous* 

printf  ("copy  mission .output . l_second  output . l_second.previous\n* 

s>3te:r  ■."copy  mission  .  c-tput .  l_second  output  .  l_second  .previous ' 

1 1  ( _^CAT  I  OImLAB  ; 
( 

strcpy  (buffer,  "  telemetry  data  backup  complete"); 

send_data_on_virtuaI_world_socket  ();     /'  buffer  msg  sent  */ 
} 

tl-^fe   /*  don't  bother  backing  up  most  recent  results 

if  iLGCATIONLAE  UU    LOOFFOREVERj 

rewind  (auvtext f i le , ; 
rewind  (auvdataf i le ; ; 
if  (TRACE  i&  DISPLAYSCREEN) 

printf  ; " lauvtextf ile  &  auvdataf iie  rewound  to  *); 

printf  ( "output .data . previous  &  output . auv. previous] \n" ) ; 
strcpy  (buffer,  "  telemetry  data  backup  skipped"); 
send_data_on_virtual_world_socket  ();     /•  buffer  msg  sent  */ 


f flush  (auvscriptf ile) ; 

if     (fclose  (auvscriptf ile)  ==  0) 

{ 

if  (DISPLAYSCREEN) 

printf  ("[success  closing  auvscriptf ile  mission. script .backup) \n" ) ; 
) 
else   if  (DISPLAYSCREEN) 

printf  ("[failure  closing  auvscriptf ile  mission. script .backup) \n* ) ; 

f flush  (auvordersf ile) ; 
fclose  (auvordersf ile) ; 


«if  defined (sgi) 
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#e2  se 


*erjdi: 


if  (TRUE  ii  DISPLAYSCREEN) 

pr:nrf  " rm  mission . output .ordersXn" 

syscerri  ( "  rm  mission,  output  .orders" 

printf  ("mv   mission . output . orders .backup  mission. output .ordersXn' 

sy?teri  ("mv   mission . output . orders . backup  mission . output . orders' 

printf  ("del  mission. output .ordersXn" 

system  {'del  mission. output .orders" 

printf  ("copy  mission. output . orders .backup  mission. output .ordersXn' 

system  ("copy  mission. output . orders .backup  mission. output .orders" 

printf  ("del  mission . output . orders .backup\n\n* ) ; 

system  ("del  m.ission  .output .  orders  .backup"   ); 


orders 


I 


«;f  defined (sgi ) 

sprintf  (buffer,  "rm  %s\n", 
♦  else 

^-rvir.rf  ; buffer,  "del  %c\n" 
•tr-ndaf 

-f    ;ri:::FLAy£CREEN:  printf 

jysteiT:  •;  buffer  ,  ; 


. 3 \ n "  ,  bu  f  f  e  r )  ; 


AUVORDERSFILENAME) ; 
AUVORDERSFILENAMEj ; 


«if  defined !sgi ) 

sprinrf  (buffer,  "mv    %s. backup  %s\n",  AUVORDERSFILENAME,  AUVORDERSFILENAME); 
♦  else 


sprin! 


[buffer,  "copy  %3. backup  %s\n",  AUVORDERSFILENAME,  AUVORDERSFILENAME); 


«end:f 

:f  TISPLAYSCREEN'  printf   {"%s\n",  buffer); 
?v3terr,     (buffer,; 


"_  I  aei  .:.neG  :'  sgi  ) 

♦  else 

sprintf  'buffer,  "del   %s      \n", 

It     TZJFLAYSCREEK,  printf   ("%s\n",  buffer); 

system    (buffer ; ; 

*er,d:f 


AUVORDERSFILENAME) ; 


/ 


-  e-mail  --------------_-_-»/ 


«.f  def  ir.ed  (sgi  ) 

sprintf  (buffer,  "rm  %s\n", 

♦  trlS'^ 

'liir.tf  (buffer,  "del  %s\n", 
«enc^ : 

if  'DISPLAYSCREEN)  printf   !"%s\n",  buffer); 
systeiT:    (buffer )  ; 


«:f  defined'sgil 

siirinrf  (buffer,  "cp  %s 
♦  else 

sprintf  (buffer,  "copy  %s 
♦endif 

if  (DISPLAYSCREEN)  printf   ("%s\n",  buffer); 

system    (buffer) ; 


%s\n",  AUVINFOFILENAME, 
%s\n",  AUVINFOFILENAME, 


AUVEMAILFILENAME) ; 
AUVEMAILFILENAME) ; 


AUVEMA I L  F I LENAME ) ; 
AUVEMAILFILENAME) ; 


♦:f  defined(sgi) 

sprintf  (buffer,  "cat  %s  >>  %s\n",  AUVSCRIPTFILENAME,  AUVEMAILFILENAME); 
♦  else 

sprintf  (buffer,  "list  %s  »  %s\n",  AUVSCRIPTFILENAME,  AUVEMAILFILENAME); 
♦endif 

if  (DISPLAYSCREEN)  printf   CIsNn",  buffer); 

system    (buffer) ; 
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*: f  defined isgi ) 

sprincf  (buffer,  "cat   %s  >>  %s\n",  AUVORDERSFILENAME,  AUVEMAILFILENAME) ; 

^rrintf  (buffer,  "list  %s  >>  %s\n",  AUVORDERSFILENAME,  AUVEMAILFILENAME); 
«-:-:d:f 

:f  (DISPLAYSCREEN)  printf   C'csxn",  buffer); 
syjten.    (buffer )  ; 

if  (  (inr  ■  (strlen  (emai l_address)  >=  3)  &&  (EMAIL  ==  TRUE)) 
( 

sprincf  (buffer,  'mail  %s  <  %s",  eniail_address,     AUVEMAILFILENAME); 
«:f  def ined (?gi : 

printf  C'csXn",  buffer); 
syscem     (buffer) ; 

/■*  system     (buffer)  ;         /•  e-mail  not  available  directly  on  OS-9  */ 
send_daca_on_virtual_world_socket  ();     /*  buffer  msg  sent  anyway  •/ 
*endif 
) 

/*  pennit  changing  the  vehicle  mission  during  continuous  lab  testing    •/ 

:f  (LOCATIONLAE  S.4.  LOOPFOREVER ; 

\ 

get_controi_constants  (); 

previousicopclock  =  clock  i); 

rtcord_da:a_on  ; )  ; 

strcpy  (buffer,  "  Lead  mission  again"); 
send_data_on_virtual_world_socket  () ; 

/■*  buffer  containing  message  sent  */ 
) 

:    wh.le  fLOCATIONLAs  &i  LOOPFOREVER);  /*  end  of  lab  infinite  loop  (if  any)  •/ 

r--::-._r  c  t-r  7_-f  f  ,);  /*  all  dor.e,  turn  them  off  •/ 

:t     -Tr.ATE  i.i    riSPLAYSCREENy 

printf  ."iser.ding  'shutdown'  message  to  virtual  world  dynamics]  \n" )  ; 

strcpi'  (buffer,  "shutdown");         /*  must  start  with  'shutdown'  to  die  */ 
i-T:.:-_J?::  i_':._v.rr_al_worId_socket  ');  /•  buffer  containing  message  sent  */ 

=!-.utdDwn_virtual_worid_socket  i;;     /•  close  connection  to  virtual  world  */ 

close_device_paths  i ) ; 

record_data_of f  (); 

if  (TRACE  UL    BISPLAYSCREEN) 

printf  ("[finishing  main:   fflush  (stdout) ,  fflush  (stderr) ] \n' ) ; 

f flush  (stdout ) ; 
fflush  (stderr) ; 

if  (TRACE  iti  DISPLAYSCREEN)  printf  ('[main  exit:   return  (0)]\n'); 

«-f  defined  (sgi) 

if  (DISPLAYSCREEN)  printf  ("gnuplot  auv_plot_l_second.gnu\n' ) ; 

system  ("gnuplot  auv_plot_l_second.gnu" ) ;  /*  display  plotted  results  */ 
#endif 

return  (0) ;  /*  main  program  exit  •/ 

}  /*  end  main  program  block,  execution  is  complete  */ 
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*«*T»*******««*««»«*' 


■***«*******1 


**♦*****: 


r***««**«**»»* 


void  cIosed_ioop_control_module    ( ) 
i 

if     (TRACE    S<S<    DISPLAYSCREEN) 

printf    ("[start      closed_loop_control_module] \n' ) ; 


*****«*! 


•««•««»«*«»»«**«»«»*««*«««♦»»*****»»*/ 


/'  Speed  Control  ***»»«*• 

speed  =  get_speed  ( ; ; 

rpn.  =  (port_rpm_coiTmand  +  stbd_rpin_command)  /  2.0; 

clamp  (&  rpm,  700.0,  -700.0,  'rpm");   /*  bound  maximum  RPM  •/ 

.:  ,TRA:E  6,i  LIEPLAYSCREEN) 

printf  ("[clamp  (&  rpm,  700.0,  -700.0,  \"rpm\')  complete] \n' ) ; 

/'  Main_Motor  RPM  Control  •«*••**»»**»•»»•*•••***•**••***»****♦**•**••«*•*, 

/'  note  thruster  use  does  not  preclude  propeller  use  *, 

if  ,LOCATICNLAB  ==  TRUE) 

pcrt_rprri   =   port_rpm_comKiand; 
i;tDJ_i'prT    =    St  Dc_rpiT;_comrriand ; 

el=e  "  in  water,  propeller  control  */ 
t 

pcrt_rpm  =  port_motor_rpm  (); 

stLd_rprT  =  stbd_motor_rpn".  (  )  ; 


i  NCT_Y  ET_R  E : M  P  LEMENTED ) 

m5in_rriOtor_del  tal  =  fabs(rpiTi;  -  stbd_rpm; 
ma-.n_motor_del  ta2  =  fabs(rpm;  -  port_rpni; 

,"■  this  is  reset  windup  for  proportional  integral  control  of  motor  speed  */ 
/'    m  order  to  prevent  accumulating  the  integral  of  speed  error       */ 
if  !main_motor_deltal>50 . 0 )   main_motor_deltal  =  50.0; 
if  viT,ain_motor_del  ta2>60  .  0  )   main_motcr_delta2  =  50.0; 

n'iai;._motor_vol  tl  =  main_rootor_voltl  +  ( rpm/f  abs  (rpm) '0  .  2*main_motor_del  tal)  ; 
main_motor_volt2  =  main_motor_volt2  + (rpm/ f abs (rpm) *0 . 2*inain_motor_delta2) ; 

if  !main_motor_voj tl  >  1023)  main_motor_vol t 1  =  1023; 

.:  (n.a-.n_motor_vci  tl  <  0)  main_motor_vol  1 1  =  0; 

if  imain_motor_vol t2  >  1023)  main_motor_vol t2  =  1023; 

-f  iniain_nictor_vol  t2  <  0/  main_motor_vol  t2  =  0; 

dacl (main_motor_voltl,RIGHT_MOTOR) ; 
dacl (main_motor_volt2,LEFT_M0T0R) ; 

/'  if  using  virtual  world  dynamics,  network  is  source  of  values  ««««  */ 


phi  =  roll_angle 

theta  =  pitch_angle 

psi  =  calculate_psi 

p  =   roll_rate_gyro 

q  =  pitch_rate_gyro 

r  =  yaw_rate_gyro 

z  =  depth 


/*  read  roll  angle  */ 

/*  read  pitch  angle  •/ 

/*  Read  heading  */ 

/*  read  roll  rate  */ 

/*  read  pitch  rate  */ 

/*  Read  yaw  rate  */ 

/•  Read  depth  */ 


/•  note:   in  laboratory  using  virtual  world,  values  above  are  superceded  */ 
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/'  Concrcl  laws  **"   NOTE;   all  k_  constants  must  be  (  +  )  positive  •"•*  ♦/ 

waypcint_distance  =    sqrt  (   (x  -  x_coiTunand)  •  {x  -  x_command) 

+    (y    -   y_coiTunandi    *    {y    -   y_coininand)  )  ; 

/■•  use  WAYPOINTCONTROL  (not  HOVERCONTROL)  until  within  standof f_distance  •/ 
if    (; HOVERCONTROL  ==  TRUE)  &&  (waypoint_distance  >  standoff_distance) ) 
{ 

WAYPOINTCONTROL   =  TRUE; 

DEADSTICKRUDDER   =  FALSE; 

if    (waypoint_distance  >  standof f_distance  +  10.0) 

( 

port_rpm_comniand  =   700; 
stbd_rpiri_command   =    700; 
fprmtf    f  auvordersf  ile, 
'%6.jf      ?,6.1f    %5.1f    %5.1f    %5.1f    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 


.  i:\n' 


t,    psi_cominand,    x_coinmand,    y_cominand,    z_command, 

port_rpin_coinmand,    stbd_rpin_comiiiand, 

rudder_command,        planes_corrmiand, 

bow_lateral_thruster_comrnand, 

stei"n_lateral_thruster_coiTmand, 

bow_verticai_thruster_c  omnia  nd, 

stern  vertical  thruster  command) ; 


I 
else 


port_rpm_command   =    200; 
stbc_rprr_comm.and   -   200; 
fprmtf    lauvordersf  ile, 
■=-..::      "le.lf    15. if    %5.:f    %S.lf    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 
% "  .  i  f  '■  ;■  " , 

t,    psi_command,    x_command,    y_command,    z_command, 
port_rprT'_comiriand,    stbd_rpm_command, 
rudder_command,        planes_coiTaTiand, 
bow_lateraI_thruster_commrand, 
stern_lateral_thruster_comrr.and, 
bow_vertical_thruster_command, 
stern_verticai_thruster_command) ; 
) 

else   if     {(HOVERCONTROL   ==    TRUE)    &&     (WAYPOINTCONTROL   ==    TRUE)) 
/■•    restore  proper   HOVERCONTROL    */ 

WAYPOINTCONTROL      =    FALSE; 
DEADSTICKRUDDER      =    TRUE; 
rudder_command        =    0.0; 
psi_command  =   psi_command_hover ; 

pcrt_rpiT._command   =    0; 
stbd_rprri_command  =   0; 

fprintf    (auvordersf ile, 
•%6.1f      %6.1f    %5.1f    %5.1f    %5.1f    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 
%E,.lf  \n", 

t,    psi_command,    x_command,    y_command,    z_coiTunand, 
port_rpm_command,    stbd_rpm_command, 
rudder_command,         planes_command, 
bow_lateral_thruster_command, 
stern_lateral_thruster_command, 
bow_vertical_thruster_command, 
stern_vertical_thruster_command) ; 
) 

if    (WAYPOINTCONTROL   ==   TRUE) 

( 

/•  note  that  a  reversed  x,y  calling  sequence  is  necessary  */ 
/*   in  order  to  get  correct  quadrant  alignment  •/ 

waypoint_angle=normalize  (degrees  (atan2  (y_command  -  y,  x_coinmand  -  x)  ) )  ; 
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psi_comrnand  =   waypoint_angle; 

:f     iTRACEj 
I 

princf    i  "WAYPOINTCONTROL  psi_comir,and   =    %5.1f,     ',    psi_command)  ; 
printf    ("X   =    %5.1f,    y    =    %5.1f\n',    x,    y); 
) 

if     (  (FOLLOWWAYPOINTMODE    ==    TRUE)    (.(,     (HOVERCONTROL    ==    FALSE)    && 
;(  waypoinc_dastance   >  standoff_di stance)     II 

(fabs    (    2    -    z_commarid(    >   scandof  f_distance)  )  ) 
i 

if     (TRACE,!    printf     ('[  FOLLOWWAYPOINTMODE   check] ')  ; 

/•    continue  until   WAYPOINT  reached  without    further   script   orders    */ 
time_next_coiTUTiand   =    t    +    2.0    *   dt; 
) 

•iisc-    /•*    WAYPOINT   reached    */ 
( 

if     (TRACE)    printf     ("[FOLLOWWAYPOINTMODE   success,    WAYPOINT   reached]'); 
fprmtf    (auvordersf  ile, 
If      It. If    %5.1f    "oB.lf    %5.1f    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 
f  \n", 

t,    psi_cormTiand,    x_coiTimand,    y_cominand,    z_coinmand, 
po r t _r piTi_c  omnia  nd ,    s t  bd_rpiTi_c  omma  nd , 
rudder_cominand,        planes_cominand, 
bow_lateral_thruster_command, 
stern_lateral_thruster_cominand, 
bow_vertical_thruster_command, 
stern_vertical_thruster_comniand)  ; 
j 

el?e    if     (HOVERCONTROL   ==    TRUE) 
1 

waypcint_angle=normalize  (degrees  (atan2  (y_coinmand   -   y,    x_conmand   -   x)  )  )  ; 

track_angle  =  normalize  (waypoint_angle  -  psi i ; 

aicng_t racK_distance  =    cos  (radians (track_angle ) )  *  waypoint_distance; 

;rcss_t rack_distance  =  -  sin  (radians (track_angle) )  *  waypoint_distance; 

pcrr_rprT.  =    k_propeller_hover    '  along_track_distance   - 

k_surge_hover    *  u; 

?tbd_rprT'   =    k_propei  ler_hover    •  along_track_distance   - 

K_surge_hover  *  u ; 


If 


pr . nt  f  1 " HOVERCONTROL : \ n " )  ; 

printf  ( "psi_command  =  %5.1f,    ",    psi_coiTunand)  ; 

printf  ."x   -    %5.1f,    y    =    %5.1f\n",    x,    y); 

printf  I "waypoint_distance  =  %5.1f,  track_angle  =  %5.1f\n", 

waypoint_distance,         track_angle) ; 

printf  ( "along_track_distance  =  %5.1f,  ",  along_track_distance) ; 

printf  ( "cross_track_distance  =  %5.1f\n',  cross_track_distance) ; 

printf  v"port_rpn-.  /  stbd_rprri  =  %5.1f\n',  port_rpm)  ; 


} 


/*  Simplified  PD  rudders/planes  control  rules:  --------___--*/ 

delta_rudder   =        k_psi    *   normalize2    (psi    -  psi_coiTmand) 

+    (k_r     *   r)    +    (k_v     *  v) ; 

/•   canh  not  provided  under  OS-9  C,  added  at  end  of  this  prograin        */ 

if  (SLIDINGMODECOURSE  ==  TRUE) 
I 

Sigma   =   k_sigma_r   *    r   +   k_sigTna_psi    *   normalize2    (psi    -  psi_coiranand)  ; 
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delta_rjdder  =   (3.1403  *  r)  +  81.9712  *  eta_steering  *  tanh  (sigma); 
) 

depch_error   =  (z  -  z_command) ; 

/■■  ~on--crain  depch_error  to  +-  15.0  feet  to  prevent  going  vertical       •/ 
/*       and  enatle  stable  pitch  angle  even  on  large  depth  changes       •/ 

clamr  (&  depth_error,  -15.0,  15.0,  "depth.error" ) ;  /*  feet  •/ 

delta_planes  =  -  (k_z  •  depth_error) 

+  (k_theta  •  theta)  +  {k_q  •  q)  -  (k_w  «  w) ; 

/'  Dead  stick  means  no  open  loop  control  of  rudders/planes  -------*/ 

if  (DEADSTICKRUDDER) 
i 

delta_rudder  -    rudder_command; 
) 
if  ;DEAD£T I CK PLANES) 

deira_planes  =  plane£_comrriand; 
) 

/ '  ccnstram  planes  i  rudder  orders  +/-  40.0  degrees  */ 

claiTip  f&  del  ta_rudder,  -40.0,  4C.0,  "del  ta_rudder '  )  ;  /•  degrees  */ 

clanp  {^   de:  ta_p'lanes,  -40.0,  40.0,  "del  ta_planes'' )  ;  /*  degrees  */ 

/*  ?er.d  -'^TuTand?  to  rudders  and  planes  »»•»»«»•••***•*•«••****•«•«»•»•««•«/ 

rudder  (del ta_rudder ; ; 
pli::':es    (delta_planes  ■  ; 


'*  Simplified  lateral /vertical  thruster  control  rules:  ---------*/ 

li  i.ROTATErojJTROL  ==  TRUE)   /'  open  loop  rotate   thrusters  •/ 

lateraI_thruster_volts  =   k_thruster_rotate  *  rotate_cominand 

*  rotate_cominand; 

else  if  (LATERALCONTROL  ==  TRUE)  /*  open  loop  lateral  thrusters  */ 
i 

Iateral_thruster_volts  =  -  k_thruster_lateral  •  (lateraI_coinmand)  ; 
) 

else  /*  heading  control  is  default  •/ 

t 

lateral_thruster_volts  =  -  k_thruster_psi  *  nonnalize2  (psi-psi_cominand) 

-  k_th]-uster_r   *  r; 
) 

vertical_thruster_volts   =  -  k_thruster_z  *  (z  -  z_command) 

-  k_thruster_w  *  w; 

if  { (THRUSTERCONTROL)  II  (HOVERCONTROL  ==  TRUE)  II  ( ROTATECONTROL  ==  TRUE)) 
{ 

AU\'_bow_vertical   =  vertical_thruster_volts; 

AUV_stern_vertical  =  vertical_thruster_volts; 

if       (LATERALCONTROL  ==  TRUE) 

{ 

AUV_bow_lateral    =    lateral_thruster_volts;  /*  both  positive,  */ 
AUV_stern_lateral   =    lateral_thruster_volts;  /*  same  direction  */ 

) 
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else  if  (HOVERCONTROL  ==  TRUE) 
( 
AU\_bow_IaCeral    =  -  (  -  k_thruster_psa  *normalize2  (psi -psi_coinmand) 

-  k_thruster_r   «  r) 

+  k_thruster_hover   *  cross_track_distance 
+  k_sway_hover      *  v; 
AUV_srern_]ateral   =    (  -  k_thruster_psi*nonnalize2  (psi-psi_cominand) 

-  k_thruster_r   *  r) 

+  k_thruster_hover   •  cross_track_distance 

+  k_sway_hover      *  v; 
) 

else  if  !  (THRUETERCOrNTTROL  ==  TRUE)  II  (ROTATECONTROL  ==  TRUE)) 
I 

AUV_bow_lateral    =  -  lateral_thruster_volts;  /*  negative  •/ 

AlTV_?tern_lateral   =    lateral_thruster_volts; 

else 
1 

prir.tf  ("Thruscer  control  logic  error  ***  \n'); 
} 

if  {TRACE  &S.  DISPLAYSCREEN; 
( 

printf  (  "Thruster  control  ON.   Pre-clamp  calculated  values: \n"); 
printf  ;"  AUV_bow_vertical  =  %6.3f,  AUV_stern_vertical  =  %6.3f\iT, 

AU\'_bow_vertical ,         AUV_stern_vertical )  ; 
printf  ("  AUV_bow_lateral   =  %6.3f,  AUV_stern_lateral   =  %6.3f\n", 

AUV_bow_laterai ,  AU\'_stern_lateral )  ; 

} 

e2?e  /'  thrusters  disabled  */ 

if  (TRACE  ia  DICPLAYSCREErv', 

printf  I  "Tnruster  control  OFF.   Pre-clamp  calculated  values : \n" ) ; 

pMi'itf    I  "verticai_thruster_vcits   =    %6.3f\n", 
vertical_thruster_volts) ; 

printf  (  " lateral_thruster_volts  =  %6.3f\n", 
lateral_thru3ter_volt£ J ; 
) 

AU"v'_bow_vt  rtical  =  0.0, 
AU'y_stern_verticai  -  0.0, 
Au\'_bow_lateral  =  0.0, 
AU"v_3tern_lateral   =   0.0, 

if  (TRACE  ii  DISPLAYSCREEN) 
{ 

printf  ("Pre-sqrt  thruster  control  calculated  values : \n' ) ; 

printf  ( "AUV_bow_vertical   =  %6.3f\n",  AUV_bow_vertical ) ; 

printf  ( 'AUV_stern_vertical  =  %6.3f\n",  AUV_stern_verticai ) ; 

printf  (•AUV_bow_lateral    =  %6.3f\n',  AUV_bow_lateral) ; 

printf  ( ■AUV_stern_lateral  =  %6.3f\n",  AUV_stern_lateraI) ; 
) 

/•  convert  to  signed  sqrt  to  account  for  volts-to-thrust  relationship 

AUV_bow_vertical   =  2.0  *  sqrt (6.0)  *  sign  (AUV_bow_vertical   ) 

*  sqrt  (fabs  (AUV_bow_vertical   )); 
AUV_stern_vertical  =  2.0  *  sqrt (6.0)  *  sign  (AUV_stern_vertical ) 
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AlT\'_bow_lateral    =  2 
A'."'  -^r^i'r._lareral   =  2 


*  sqrt  (fabs  {AUV_stern_vertical 
sqrc(6.0)  *  sign  (AU\'_bow_lateral 

'  sqrt  (fabs  {AUV_bow_lateral 

sqrt (6.0)  '  sign  (AUV_stern_lateral 

*  sqrt  (fabs  (AUV_stern_lateral 


It 
i 


(TRACE  iS,  DISPLAYSCREEN) 

prantf  ("Post-sqrt  thruster  control  calculated  values : \n" ) ; 

printf  ( ■AUV_bow_vertical   =  %6.3f\n",  AUV_bow_vertical ) ; 

printf  ( "AUV_stern_vertical  =  %6.3f\n',  AUV_stern_vertical ) 

printf  ( •AUV_bow_lateral     =  %6.3f\n",  AUV_bow_lateral ) ; 

printf  ( ■AUV_stern_lateral   =  %6.3f\n",  AUV_stern_lateral ) ; 


constrain  thruster  orders  +/-  24.0  volts  ==  3820  rpm  no-load 


:.a-ip 


c  1  amp 

'i, 

C.a.T.p 

,u 

clanip 

(& 

AUV_bow_vert jcal ,  -24.0, 

AUV_stern_vertical ,  -24.0, 

AUV_bow_later3l,  -24.0, 

AUV_stern_lateral,  -24.0, 


2  4.0,  •AUV_bow_vertical ") ; 

24.0,  •AUV_stern_vertical • ) 

24.0,  •AUV_bow_lateral' ) ; 

24.0,  •AUV_stern_lateral" ) ; 


if    (  (port_rpni_comniand   ==    0.0)    &i    (stbd_rpiTi_corrunand  ==   0.0) 
/'   prevent   planes   chatter   by    zeroing   their,  at    zero  speed 


^TZ-  -    ^  G 


.ruaoei    = 
dfe- ta_piaries    = 


0.0; 
0.0; 


/*  Ncrrriali  zation  within  bounds  ---------------------•/ 

d^lca_rudder  =   normalize2  (del ta_rudder i ; 
d.rl tc_j:-lanes  =   normalize2  (del ta_planes)  ; 

.' *  constrain  planes  &  rudder  orders  *-  40.0  degrees  */ 

i:  '  ihijs    (del  ta_rudder ;  >  (40.  C  /*  degrees  */)) 

( 

delta_rudder   =  (40.0)  *  {delta_rudder  /  fabs  (delta_rudder) ) ; 
) 
if  (fat)S  (delta_planes)  >  (40.0  /•  degrees  */)) 

deita_planes   =  (40.0;  '  (delta_planes  /  fabs  (delta_planes) ) ; 
} 

/'  Send  comiTiands  to  rudders  and  planes  **•••***♦«•**•**•«•*•••••••*•♦**»••/ 

rudder  (delta_rudder ) ; 
plane?  idel ta_pl anes ) ; 

/•  Send  command  &  get  reply  from  sonar  *»*»*******»•*****««***«•*•»*•*****/ 

A'J'._£T:uUu_bfearing  =  0.0;  /'  relative  bearings  of  sonar  heads  •/ 

AUV_£T72  5_beari.ng   =  0.0; 

/*  send  telemetry  to  tactical  level  and  data  recording  files  ------*/ 

record_data  ( ) ; 

/•  read  commands  from  tactical  level  -------------__-__•/ 

if  (TACTICAL  ==  TRUE)  read_parallel_port  (); 

/•  update  simulation  clock  "f  --------------__-____*/ 

t  =  t  +  dt ; 

fflush  (stdout); 
currentloopclock  =  clock  (); 
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if  f (REALTIME  ==    TRUE)  &&  LOCATIONLAB  && 
(currencloopclock  <   previ ousloopclock 

+  (int)(dt  •  (float)  CLOCK£_PER_SEC) ) ) 
I 

if  (TRACE  &&  DISPLAYSCREEN) 
{ 

princf  ( "currentloopclock  =  %ld,  previ ousloopclock  =  %ld\n", 
currentloopciock,      previousloopclock) ; 
princf ( "cimescep  dt  =  %5.3f  seconds  (corresponding  clock  ticks  =  %d)\n', 
dt,  (int)  (dt  •  (float)  CLOCKS_PER_SEC) )  ; 
princf  ("Busy  wait  until  system  clock  reaches  simulation  clock,  • ) ; 
print f  ("loop  duration  =  %5.3f\n", 

((float)  currentloopciock  -  (float)  previousloopclock) 
/  CLOCK£_PER_SEC) ; 
) 
while   (currentloopciock  <  previousloopclock 

+  (int) (dt  •  (float)  CLOCKS_PER_SEC) ) 
{ 

currencloopclock  =  clock  ();  /•  %%%%%  busy  wait  %%%%%  */ 
) 

if  (TRACE  US.    DISPLAYSCREEN) 
{ 

printf  ("Busy  wait  complete,  loop+wait  duration  =  %5.3f,  ", 

((float)  currentloopciock  -  (float)  previousloopclock) 
/  CLOCKS_PER_SEC) ; 
printf  ("current  clock  ()  =  %ld\n',  currentloopciock); 
} 

e:.-e  if  ((REALTIME  ==  FALSE)  6<i<  LOCATIONLAB  &&  DISPLAYSCREEN  &&  TRACE) 

printf  ("No  busy  wait,  loop  duration  =  %5.3f,  ", 

(  (float)  currentloopciock  -  (float)  previousloopclock) 
/  CLOCKS_PER_SEC) ; 
printf  ("current  clock  ()  =  %ld\n",  currentloopciock); 
J 
previousloopclock  -    clock  (); 

/  ■"  T'Stimace  X  and  Y  by  dead  reckoning  -----------------*/ 


X  =  X  -  [sp^'iz    *  dc  *  CO?  (radians  (psi))); 
y  =  y  *  (speed  '  dt  *  sin  (radians  (psi))); 

if    ifecf  (auvscriptf ile)  &&  (t  >=  time_next_command) )  /*  all  done  */ 
{ 

if  (TRUE  ii  DISPLAYSCREEN)  printf  ( "end_test  set  TRUE\n"); 

end_ce?r  =  TRUE; 
I 

else  if  (t  >=  time_next_command)  /*  scriptfile  not  yet  closed,  read  more  •/ 
< 

if  (TRUE  icU    DISPLAYSCREEN) 

princf  ("\n[read  more  from  parse_mission_script_commands] \n' ) ; 

parse_mission_script_commands  ();    /•  get  next  script  orders  read  */ 
) 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  closed_loop_control_module  {)]\n"); 

return; 

)  /*  end  closed_loop_control_module  ()  */ 


void  get_control_constants  () 

{ 

if  (TRACE  it  DISPLAYSCREEN) 


/*  get  data  from  file  at  program  start  */ 
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prinrf  ('[start  get_control_constants  ()]\n'); 


if  (ENTERCONTROLCONSTANTS)     /■.__--.--_--. 
( 

printf i ' Inpuc  start_dwell \n" ) ; 

scanfC%d",    s.start_dwell )  ; 

/*  note  %F  required  by  OS- 9,  accepted  by  SGI  •/ 
printf (" Input   k_psi ,  k_r,  k_v\n"); 
scanf("%F  %F  %F",  &k_psi ,  &k_r,  &k_v) ; 

printf (" Input  k_z,  k_w,  k_theta,  and  k_q\n"); 
scanf("%F  %F  %F  %F",  &k_2,  &k_w,  &k_theta,  S.k_q)  ; 

printf  ! " Input  k_thruster_psi , k_thruster_r\n' ) ; 
scanf("%F  %F',  &k_thruster_psi ,  &k_thruster_r) ; 

printf ( *  Input  k_thruster_rotate\n" ) ; 
scanf  i  ''oF'  ,  6<k_thruster_rotatei  ; 

printf ( " Input  k_thruster_z , k_thruster_w\n" ) ; 


V 


SCdIil 


%F 


&k_thruster_z ,  &k_thruster_w) ; 


printf (" Input  k_propel ler_hover ,  k_surge_hover\n' ) ; 
scanf('%F  %?' ,    &k_propeller_hover,  &k_surge_hover) ; 

printf I " Input  k_thruster_hover ,  k_sway_hover\n" ) ; 
scanf'"%F  %F",  £,k_thruster_hover , 
S<k_!;way_hcver )  ; 

printf  1  • Input  speed_limit  from  1  to  2  feet/sec  \n"); 
scanf  '  "IF" ,  S.speed_lim:  t )  ; 

printf  ( "Input  rpir,  from  +-200.0  to  +-700.0\n"); 
scanf ( "%F" , irpm, ; 


else  /'  use  default  initialization  values 


if  (TRACE  &i  DISPLAYSCREEN) 

printf  ('(using  default  control  constant  values] \n* ) ; 


st5rt_dwell  = 

k_psi 

k_r 

k_v 

k_2 
k_w 

k_theta     = 
k_q 

rpm 


L ;  /*  delay  time  in  seconds  */ 

1.00;  /*  degrees  rudder  per  degree  of  course  error  */ 

2.00;  /■'  degrees  rudder  per  degree/sec  yaw  rate    •/ 

COO;  /*  needed  ??  »/ 


15.0 
2  .0 
4  .0 

1  .0 


/*  degrees  planes  per  foot  of  depth  error 


'/ 


=  400.0; 


k_thruster_psi  = 
k_thruster_r  = 
k_thruster_rotate  = 

k_thruster_lateral= 

k_thruster_z  = 
k_thruster_w      = 


0.6;  /*  volts  per  1  degree  course  error       •/ 
5.0; 

2.25;  /*  (24V)-2=>  2  #  =  16.0  deg/sec  empirical*/ 

/•  k_thruster_rotate=  (24V  /  16  deg/sec) '^2'/ 

48.0;  /•  24  V  =  2  #  =  0.5  ft/sec  empirically  V 

/*  note  voltage  follows  a  square  law     */ 

50.0;  /*  guesses  •/ 
50.0; 


k_propeller_hover  =  200.0;  /*  200  rpm  per  one  foot  error  */ 

k_surge_hover     =  6000.0;  /*   60  rpm  per  0.01  foot/sec  surge      */ 
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/*  this  value  is  high  to  reduce  sternway  */ 

k_thi'uster_hover   =    4.0; 
k_sway_hover      =    4C.0; 

i 

3peed_]imit  =     2.C;       /•  1.0  to  2.0  ft/sec  •/ 

if  (TRACE  &&  DISPLAYSCREEN) 
( 

prmtf  ("[k_psi  =  %5.2f,  k_r  =  %5.2f,  k_v  =  %5.2f,  k_z  =  %5.2f,  ", 

k_psi,         k_r,        k_v,         k_z) ; 
printf  ("k_w  =  %5.2f,  k_theta  =  %5.2f,  k_q  =  %5.2f]\n", 

k_w,         k_theta,         k_q) ; 
printf  (• [k_thruster_psi  =  %5.2f,  k_thruster_r  =  %5.2f,  ", 

k_thruster_psi ,         k_thruster_r) ; 
printf  (" [k_thruster_rotate  =  %5.2f,  ", 

k_thruster_rotate) ; 
printf  ( "k_thruster_z  =  %5.2f,  k_thruster_w  =  %5.2f]\n", 
k_thruster_z,         k_thruster_w) ; 

printf  ( "k_propeller_hover  =  %5.2f,  k_surge_hover  =  %5.2f]\n", 
k_propeiler_hcver ,         k_surge_hover ) ; 

printf  ■; "  k_thruster_hover  =  %5.2f,   k_sway_hover  =  %5.2f]\n", 
K_tnruster_hover ,  k_sway_hover ) ; 

.f  ■  controlconstantsfiie  =  fopen  (CONTROLCONSTANTSFILENAME, "w" ) )  ==  NULL) 

printf !°AUV  execution:   unable  to  open  output  file  %s  for  writing.   ", 

COrJTROLCONETANTSFILENAME)  ; 
printf 

;"  Check  ownership  perrrussions  in  current  directory .  \n" )  ; 

piinr  :  (  "Exi  t .  \n"  )  ,- 
exi t   ( - i  ;  ; 

If  .TRACE  l,i>    EiISFLAYECREEN) 

printf  ;' [controlconstantsfiie  %s  open,  pointer  =  %x]\n", 
CONTROLCONSTANTSFILENAI-IE,  controlconstantsfiie)  ; 

:p:..ntf  ;cc  ntrolconstantsfile, 

•'  \n\n"); 

t printf  (controlconstantsfiie, 

"  AUV  execution  level  control  algorithir.  coefficients  \n"); 
fprintf  (controlconstantsfiie, 


\n\n\n") ; 


fprintf  (controlconstantsfiie, 

k_psi   k_r    k_v    k_z    k_w    k_theta     k_q  \n\n'); 
fprintf  (controlconstantsfiie, 

"  %5.2f    %5.2f   %5.2f   %5.2f   %5.2f    %5.2f      %5.2f  \n\n\n\n', 
k_psi,    k_r,    k_v,    k_z,    k_w,    k_theta,     k_q  ); 

fprintf  (controlconstantsfiie, 

k_thruster_psi      k_thruster_r       k_thruster_rotate\n\n' ) ; 
fprintf  (controlconstantsfiie, 

%5.2f  %5.2f  %5.2f  \n\n\n\n", 

k_thruster_psi ,     k_thruster_r,      k_thruster_rotate) ; 

fprintf  (controlconstantsfiie, 

k_thruster_z        k_thruster_w  \n\n"); 
fprintf  (controlconstantsfiie, 

%5.2f  %5.2f    \n\n\n\n", 

k_thruster_z,       k_thruster_w) ; 

fprintf  (controlconstantsfiie, 

k_propeller_hover   k_surge_hover  \n\n'); 
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fprir.tf  (contrclconstancsfile, 

%5.2f  %5.2f     \n\n\n\n", 

k_propelIfer_hover ,   k_surge_hover ) ; 

fprintf  (controlconstantsf ile, 

k_thruster_hover    k_sway_hover  \n\n"); 
fprintf  (controiconstantsf i le, 

%5.2f  %5.2f  \n\n\n\iT, 

k_thruster_hover ,        k_sway_hover) ; 

f flush  (control constantsfile) ; 
fclose  (controlconstantsf ile) ; 

if  'TRACE  i-i.  DISPLAYSCREEN) 

piinLf  ("[finish  get_cont rol_constancs  ()]\n'); 

return; 

/■*    end   get_controI_constants    ()    '/ 


doutle   aef'th    (; 

inr  val  =    0; 

doucle   new_2  =    0.0; 

jcutle    2_cff?et    =    O.C; 

if      TRACE   i.&    r  ISPLAYSCREEN;    printf     ("  [start      depth    {)]\n"); 

if     ;L0CAT10NLA£    ii    DEADRECKON) 

z    =    2_coimiand; 
) 

if     (N^T_YET_RE!M?LEKE^^^^ED) 

2_cffset    -    0.0; 

vil  =    adc2  '0, Oy ; 

riew_2  =    0.002237    '    (val    -    2_valO)    -^    z_offset;  /•    new_z    (ft)    */ 

elsc   ne-^_2    =    2; 

If  (TRACE  £.i  DISPLAYSCREEN) 

printf  ("[finish  depth  (),  returns  %5.3f]\n",  new_z); 

r-^t  jrn  (  new_z  i  ; 

;  /'  end  depth  ( )  */ 

double  calculate_ps:  () 
{ 

unsigned  short  psi_bit; 

int  psi_bit_int,psi_bit_old_int,delta_psi_bit; 

double  angle, tpi ; 

double  pi  =  3.1415927; 

if  (TRACE  &£<  DISPLAYSCREEN)  printf  ("[start  calculate_psi  ()]\n'); 

if  (LOCATIONLAB  &&  DEADRECKON) 
{ 

psi   =  psi_coiranand; 
} 
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f     ;NCT_YET_PEIMPLEMEr>ITED) 


*    i-orr    Vir^ed:    -C   be   redone:       ♦/ 
psi_bU    =   Read_PortAB( (struct   MFI_PIA   *)    MFI_BASE) ; 

psi_bJt    4.=    Ox3FFF; 
psi_bit_int    =   psi_bit; 
psi_bit_old_int    =   psi_bit_old; 

delta_psi_bi t    =   psi_bit_int    -   psi_bi t_old_int ; 
p?i_bit_old   =   p£i_bit; 

if iabs (del ta_psi_bit )    >    10000) 
1 

wrap_count    =   wrap_count    -   del ta_psi_bit/abs (del ta_psi_bit ) ; 
) 
tpi    =    2 . 0    *    pi    *   wrap_count; 

angle   =   heading i )    -   dg_offset    +    tpi ; 

angle    =   degrees    (angle); 

elstr    angle   =   psi  ; 


.:     7?j-.:-E  ii  displayscreen; 

printf  ("[finish  calculate_psi 

return  (normalize  (angle)); 

■'  end  calcuiate_psi  ()  */ 


returns   %5.3f]\n",    angle); 


■  -r  t  ■»   w  ■*   T  • 


■•****«********«*«**«************«******«« 


ze  1  c-^g'^  i  c J  a  t  a    ;  ; 

m:    mdtrx; 

-.ni    save_trace; 

save_trace    =    TRACE; 

if     .TRACE   &i    laSPLAYSCREEN)    printf    ("[start      zero_gyro_data    ()]\n' 


pitch_0 
roj.l_0 


=    add  (6) 
=    add  (7) 


roll_rate_0      =   add  (9) 
pitch_rate_0   =   add  (8) 
yaw_rate_0        =    add  (10); 
z_valC  =    adc2 (0,0) ; 

dg_offset  =    heading(); 

If  (TRACE  &S,  DISPLAYSCREEN) 

printf  ('[device  averaging  for  2  seconds...  ]\n'); 
for  (index=0; index<99; ++index) 


{ 


pitch_0  +=   add  (6) 

roll_0  +=   add  (7) 

roll_rate_0  ••■=   add  (9) 

pitch_rate_0  +=   add  (8) 

yaw_rate_0  +=   add  (10); 

2_val0  +=    adc2 (0,0) ; 

TRACE  =  FALSE; 

dg_offset  +=  headingO;  /*  this  is  verbose  if  TRACEd  •/ 

TRACE  =  save_trace; 

tsleep  (5);  /*  256ths  of  a  second        */ 
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pitch_0  =  pitch_0/100; 

roli_0  =  roll_0/100; 

roll_rate_0  =  rol l_rate_0/100 ; 

pirch_rate_0  =  pitch_race_0/100; 

yaw_rate_0  =  yaw_race_0/100; 

z_va]C  =  z_vaiO/100; 

dg_offsec  =  dg_of f set /lOO . 0 ; 


if 
i 


(TRACE  Ui.    DISPLAYSCREEN) 


print  f 
printf 
pi:ntf 
printf 
printf 
printf 
pri:itf 


'pitch_0  =  %d\n" 

"roll_0  =  %d\n" 

'roii_rate_0  =  %d\n" 

"pi tch_rate_0  =  %d\n" 

■'yaw_r3te_0  =  %d\n" 

•z_valC  =  %d\n" 

'dg_offsfet  =  %f  ;n" 


pitch_0) ; 
roll_0) ; 
roll_rate_0) ; 
pitch_rate_0 ) 
yaw_rate_0 ) ; 
z_valO ) ; 
dg_of f set ) ; 


/■ 


if     [TRACE   4.6.    DISPLAYSCREEN)    printf 
refjrn ; 

'    end    zerc_gy ro_data    {)    */ 


finish   zero_gyro_daca    ( ) ] \n" ) 


r«*«»1t«***«««T»««****«i 


/ 


void  in: tiaii ze_dacs  ()  /* 

{ 

it     '.TRACE  ii,  DISPLAYSCREEN)  printf  ( 

if  'NOT_YET_REIMPLEMENTED) 
{ 

controi_surface  ( FRONT_RUD_TOF , 0 . 0 j ; 

contrcl_surface  (FRCNT_RUr_BOT, 0 .0) ; 

controI_surface  (FRONT_FL_RIGHT, 0 . 0) 

ccntrol_surface  (FRCNT_PL_LEFT, 0 .0) ; 

control_surfacfr  (REAR_RUL_TOF, 0 . 0) ; 

ccntro:_surf act  (REAR_RUD_BOT, 0 . G ) ; 

cent rol_sur face  (REAF_PL_RIGHT, 0 .Oj ; 

contrGl_surf ace  (REAR_PL_LEFT, 0.0) ; 


Initialize  all  dac  channels  to  zero  •/ 
'(start   initialize_dacs  ()]\n*); 


ir.air.  met  or s 


.f  f 


) 


if  (TRACE  4.4.  DISPLAYSCREEN)  prmtf 
return ; 
)  /*  end  initialize_dacs  •/ 


finish  initialize_dacs  ()]\n*); 


/' 


r*««*«***«i 


■*«»»♦«*****• 


*******  1 


•  •*********«*«*1 


r  *****  1 


void  initialize_adcs  () 
{ 

if  (TRACE  &S,  DISPLAYSCREEN)  printf  ("(start   initialize.adcs  ())\n'); 

«if  defined  (sgi) 
ielse 

/*  Initialize  MFI  channels:   0  =  input  port,  1  =  output  port  */ 

Init_PortA  ((struct  MFI_PIA  *)  MFI_BASE,  MFI_INPUT_PORT) ; 
Init_PortB  ((struct  MFI_PIA  *)  MFI_BASE,  MFI_INPUT_PORT) ; 

*endif 
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,f  (TRACE  s.i,  DISPLAYSCREEN)  princf  (-[finish  initial  ize_adcs  i)]\n"); 
return; 
i  ,'  end  jn: tiali 2fc_adcs  */ 

^' »»*««*♦*•«**»*********■»*******«***«****♦»*«*«**«********«*«****»*♦******«****♦/ 

void  control_surf ace  (surface,  angle) 

/*  This  function  sends  the  desired  ANGLE  to  the  specified  control  SURFACE 
The  angle  is  first  normalized  to  (-45  to  45),  then  correction  is  applied 
for  the  non-linearity  in  the  servo  control  module  */ 

int    surface; 

double  angle; 

.nt  volt; 
dCoi.ie  a,b,c,d; 

if  (FALSE  5,i,  DISPLAYSCREEN)  printf  ("(start   control_surf ace  ()]\n"); 

if  ;NCT_YET_RE IMPLEMENTED) 

a  =  :  .246"'e-4  ; 

»:.  =  -  2  .  r--  ij  c  ~  fr  -  2  ; 

d  =  500.6576; 

angle  =  angle*57 . 295779;  /*  Convert  RADIANS  to  DEGREES  */ 

:t    '(angle  -  -22.92)  II  (angle  >  22.92)) 

/'  Plane  saturated  set  tc  +-  45  */ 
angle  -    22 . 92*anglfe/fabs tangle : ; 

volt  =  a*pow (angle, 3 . )  +  b*pow (angle, 2 . )  +  c*angle  +  d; 
dac2b (volt , surf ace) ; 

) 

if  (FALSE  6.&  DISPLAYSCREEN)  printf  ("[finish  control_surf ace  ()]\n"); 

return; 
;  /*  cor.trol_surf ace  */ 

void  rudder  (angle) 

/•  Send  angular  deflection  (RADIANS)  to  rudders. 

Convention  (+)  angle  left  turn,  (-)  angle  right  turn  */ 

double  angle; 
I 

if  (TRACE  ii  DISPLAYSCREEN)  printf  (• [start   rudder  ()]\n"); 

control_surface  (FRONT_RUD_TOP, -angle) ; 

control_surface  (FRONT_RUD_BOT, angle) ; 

control_surface  (REAR_RUD_TOP, angle) ; 

control_surface  (REAR_RUD_BOT, -angle) ; 
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:f  'TF-ATE  S.6,  DISPLAYSCREEN;  prmcf  {'[finish  rudder  ()]\iT); 
return; 
}  /'  rudder  */ 

vcid  f'Snes  (angle) 

/ '  Ser.d  ai-iQular  deflection  (RADIANS)  to  bow  and  stern  planes. 
Convention  '->■)  angle  dive,  (-)  angle  rise  */ 

double  anale; 

if  (TRACE  4.S-  DISPLAYSCREEN)  printf  ("(start   planes  ()]\n"); 

ccn: rol.surf ace  (FRONT_PL_RIGHT, angle) 

control_surface  (FRONT_PL_LEFT, -angle) 

contrcl_surf ace  (REAR_FL_RIGHT, -angle) 

contrcl_surf ace  (REAR_PL_LEFT,   angle) 

it    (TRACE  i-i  DISPLAYSCREEN)  printf  C  [finish  planes  ()]\n"); 

return; 

J  / •  end  planes  ( )  */ 

void  T.5ir:_mot  crs_cf  f  ()  /•  Turn  off  both  main  motors  */ 

.t      TRACE  ir-,  I  ISPLAYSCF.EEN,  printf  ("[start   main_motors_of  f  ()]\n"); 

if  ;NOT_YET_RE:^!FLEME^JT■ED) 

daci  ;  5 12, SUPPLY)  ; 

da  r :  (512, R IGHT_MDTOR ) ; 

da c :  '  5 1 2 , LEFT_MOTOR )  ; 

.:   TRACE  i,L    :  ISPLAYSCREEN)  printf  ("[finish  main_motors_of  f  ()]\n'); 

return; 


i  /'  end  iT,ain_motors_of  f  ( 


«  / 


void  alive  (interval,  locai_start_dwell ) 

unsigned  int  interval; 

int  local_start_dwell ; 
{ 

unsigned  int  iinterval , jinterval ; 
double  test_delta; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start   alive  ()]\n"); 

if  (NOT_YET_REIMPLEMENTED) 
{ 

local_start_dwell  =  local_start_dwel 1*100; 

interval  =  interval*100; 

iinterval  =  local_start_dwell/interval ; 

jinterval  =  0; 

test_delta  =  .4;  /*  Deflect  22.5  degrees  */ 
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) 


1 


while ( iinterval  <    iinterval) 

cor.c:c:_surfdCt-  (  FRONT_RUD_TOP,  test_delta)  ; 
Csieep ( interval ) ;     /•  256ths  of  a  second  */ 
tesi_delta  =  -test_delta; 
^interval  =  jinterval  +  1 ; 


tsiefep(200) ;  /•  256ths  of  a  second  */ 

if  (TRACE  &4>  DISPLAYSCREEN)  printf  ('[finish  alive  ()]\n") 

return; 


t***«****»»***************«*««««**«** 


«*«»•*«**•»««****««»«»•**«*«*•«/ 


void  i-ecord_data_on  () 
t 

if  (TRACE  ii,  DISPLAYSCREEN)  printf  ('[start   record_data_on  ()]\n'); 

/*■  Open  files  for  writing  */ 

if  ■ (auvdatafile  =  fopen  ; AUVDATAFILENAME, 'w' ) )  ==  NULL) 

print f  '  "ALT^v'  execution:   unable  to  open  output  file  %s  for  writing. 

ALTVDATAFILENA.ME)  ; 
printf 

("  Check  ownership  permissions  m  current  directory . \n" ) ; 

p  r  I  n  t  f  ( "  Ex  i  t  .  \  n " )  ; 
e>:::   (  -  1  ,  ; 
1 
if  :  TRACE  6,i  LISPLAYSCREEN  j 

printf  ' " I auvdataf i le  %s  open,    pointer  =  %x]\n", 

AUVDATAFILENAME,  auvdataf ile) ; 

if  i (auvtextfile  =  fopen  (AUVTEXTFILENAME, 'w' ) )  ==  NULL) 
{ 

printf ('AUV  execution:   unable  to  open  output  file  %s  for  writing. 

AUVTEXTFILENAME) ; 
printf 

("  Check  ownership  permissions  in  current  directory  . \n' ) ; 

printf ( "Exit . \n" ) ; 
exit   (-1 ) ; 
) 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  I " [ auvtextf lie  %s  open,  pointer  =  %x]\n', 

AUVTEXTFILENAME,  auvtext f i le) ; 

fprintf  (auvtextf ile,  '*  auvtextfile  %s  shows  state  ',  AUVTEXTFILENAME); 
fprintf  (auvtextfile,  "vector  variables  at  one  second  intervals . \n\n" ) ; 

if  (LOOPFOREVER) 

fprintf  (auvtextfile,  '#  Mission  replication  «%d\n", 

replication_count ) ; 

/*  testing  code  from  wr2tl.c,  not  currently  in  use        */ 
/•  serial. d  is  a  telemetry  test  file  to  check  connectivity  */ 
/*   if  ( (serialtestf ile  =  fopen  ('serial. d",  'r'))  <=  0) 
{ 

printf ("AUV  execution:   record_data_on:  can't  open  test  file  serial .d\n" ) ; 
printf ( "Exit. \n" ) ; 
exit   (-1); 
) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  record_data_on  ()]\n'); 
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return; 


»«««**«*«*****r 


[«««««*«**«««»«•*«•«»**»******••***»**»*** 


void  record  data  off 


if       (TRACE   6ri<    DISPLAYSCREEN)    princf    ("(start      record_data_of f    ())\n"); 

if   (TRACE  £,&  DISPLAYSCREEN) 
( 

printf  I" [flushing  and  closing  auvdatafile  %s   %x)\n", 

AUVDATA FILENAME,  auvdatafile); 

f flush  (stdout ) ; 
) 

if   (auvdatafile  !=  NULL) 
{ 

if  (TRACE  i&  DISPLAYSCREEN)  printf  ('[auvdatafile  f lushed] \n' ) ; 

f flush  (stdout ; ; 

f flush  (auvdatafile); 

fclose  (auvdatafile); 

if  (TRACE  &i  DISPLAYSCREEN)  printf  ("[auvdatafile  closed] \n"); 

f flush  (stdout ) ; 
) 
else  if  (TRACE  ii  DISPLAYSCREEN)  printf  ("[auvdatafile  was  not  open!!]\n"); 

if   (TRACE  Sri  DISPLAYSCREEN) 

printf  '"[flushing  and  closing  auvtextfile  %s  %x]\n*, 

AUVTEXTFILENAME,  auvtextfile); 

f flush  (Stdout ) ; 

) 

(auvtextfile  !=  r^TJLL) 

if  'TRACE  iri<  DISPLAYSCREEN)  printf  ("[auvtextfile  f  lushed]  \n"  )  ; 

f  flush  f stdout I ; 

f flush  [auvtextfile) ; 

fclcst  lauvtext f lie) ; 

if  (TRACE  6.5.  DISPLAYSCREEN)  printf  ("[auvtextfile  closed]  \n"); 

f f lusn  I stdout ; ; 
i 
else  if  (TRACE  6<i  DISPLAYSCREEN!  printf  ("[auvtextfile  was  not  open!!]\n"); 

fciose  { serial testf lie ) ;   /•  serial  port  test  file  •/ 

if  (TRACE  I.U    DISPLAYSCREEN) 


I 


printf  ("[finish  record_data_of f  ()]\n'); 
f flush  (stdout ) ; 


return; 

) 

/  •  «  *  1 

«  * 

void 

record. 

.data 

0 

{ 

****«« 


►»«»*»**•««•«*»»»»«»«*«»•««•»»»»««*««««««««•««««««««««/ 


/*  temporary  hold  variables  */ 

double     AUV_time_temp, 
AUV_x_temp, 
AUV_phi_temp, 
AUV_u_teinp, 
AUV_p_temp, 
AUV_x_dot_temp , 


AUV_3'_teinp, 
AUV_theta_teinp , 
AUV_v_teiTip, 
AUV_q_temp, 
AUV_y_dot_temp , 


AUV_2_teinp, 
AUV_psi_teinp, 
AUV_w_teiiip, 
AUV_r_tenip, 
AUV_2_dot_temp , 
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AU"v'_phi_dot_temp,  Airv'_checa_dot_teinp,  AlPv'_psi_dot_temp, 

AUV_de]  t:a_rudder_temp,  AUV_del  ta_planes_temp, 

ALV_pcrt_rpm_temp ,  AUV_stbd_rpiT;_teinp, 

Arv_bow_vfeiLical_teiT,p,  AUV_sterrj_vert  ical_temp, 

AUV_bow_lateral_temp,  AUV_stern_lateral_temp, 

AUV_£T1 000_range_temp ,  AUV_ST72  5_range_teinp , 

AUV_ST1000_bearing_temp,  AUV_ST72  5_bearing_teinp, 
AU\'_ST1000_strengch_temp,AUV_ST72  5_strengch_temp; 

if    (TRACE   &£,    DISPLAYSCREEN)    printf    ("(start      record_data    ()]\n"); 

?ysterT._t  iiTie  =    time    (NULL)  ; 

systen-._tmp  =    localtime    (S<systerr,_tinie)  ; 

if  ;TRACE  ii,  DISPLAYSCREEN) 

printf  '"iOS-9  systerri  time  is  %02d :  %02d:  %02d,  replication  %d]\n", 

system_tmp->tm_hour ,  system_tmp->tm_min,  system_tmp->tm_sec , 
repi:cation_count ; ; 

*o''  'i  =  C"  i  '-    255'  '-t-+) 

buffer  \i]    =    '\(!';  /-    zero  buffer  ♦/ 
} 

buff.rr_?i2e  -   sprintf  (buffer, 
"  ajv_rtate  ^d.'if    !J5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
%5.3f  li.3t    %5.3f  %5.3f  %B.3f  %5.3f  %£.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
%E.3f  "cE,.3f  %5.3f  %5.3f  %5.3f  %5.3f  %£.3f  %B.3f  \n", 

t , x,y , z , 

r.oiTT.a^  .ze2  (phj   )  , 

ncrTriaiize2    (theta.i, 

norrrialize      (psi      j  , 

u  ,    V  ,    w , 

ncrmalize2     (p) , 

normai jze2    (q; , 

I'lcrma^  i  ze2     (  r  ;  , 

x_dot,    y_dot,    z_dot, 

ncrTriaIize2    (phi_dot      ), 

norTr,alize2    (theta_dot), 

ncnrialize2    {psi_dot      )  , 

nonTiaIize2     (delta_rudder '>  , 

ncnna2ize2    idelta_plane? ) , 

pcrt_rprr,,  stbd_rpm, 

AL^'_bcw_vertical ,  Airv/_stern_vertical , 

AuV_bow_iateral ,     AUV_9tern_lateral , 

Airv'_STlCOO_bearing,      AUV_ST1000_range,        AUV_ST100  0_strength, 

A'JV_ST72  5_bearing,    AUV_ST725_range,    AUV_ST725_strength)  ; 

if  ibuffer_size  >  254)  /•  sprintf  buffer  overflow  condition      */ 

{ 

:f  ■ DISPLAYSCREEN) 

print:  : 'Buffer  overflow,  buffer_size  =  %d,  reduced  to  254  !!!!!!\n", 

buf fer_size) ; 
buffer_size  =      2  54; 
) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  C [buf f er_size  is  %d]\n",  buf fer_size) ; 
/■*  other  state  variables  &  timing  constraints   «««««««««««««  */ 

send_data_on_virtual_world_socket  ( ) ;  /*  there  it  goes  */ 

if  (TRUE  &&  DISPLAYSCREEN)  /*  telemetry  report  to  screen        */ 

{ 

printf  ("Nnsending  to  virtual  world:"); 
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prinrf  (■\r.%s',  buffer); 
elj;^  :f  (DISPLAYSCREEN )  /*  partial  telemetry'  report 

piinrf  :  •  \n?ent  telemetry-  to  virtual  world  %5.3f   *,  t )  ; 


qt:-t_3tream_f  roin_virtual_wcrld_socket 


/•  here  it  comes  •/ 


/«  update  and  output  AUV  state  variables 

/•  note  that  if  dead  reckoning  is  used,  values  will  not  change 


variatles_parsed  =  sscanf (buf f er 

■%s  %lf  %lf  %if  %lf  %lf  % 

!  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf 


_received. 

If  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf 
%lf  %lf  %lf  %lf  %lf  %lf  %lf  \n',*/ 
S  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F 


%F  %F  %F  %F  %F  %F  %F  \n", 
keyword, 

4LAUV_x_temp, 

6l  ALT\'_p  h  i  _t  emp , 

iAUV_u_temp, 

t  AUV_p_t  eiTip , 

4<  ALr-.'_x_d  o  t  _t  enp , 

iLAUv'_phi_dot_temp , 

4tA'J"v'_del  ta_rudder_tenip , 

&AU\'_pcr  t_rp]Ti_temp , 

u  A  'J.'_Sj  o  w_v  e  r  t  i  c  a  1  _  t  emp , 

6tAUV'_bow_lateral_temp, 

tA'JV_STi  O0  0_bearing_teirip 

iiAlTv  _£T7  2  5_bea  r  i  ng_t  emp , 


4LAUV_t  ime_temp , 

tAUV_y_temp, 

tAUV_t  h  e  t  a_t  emp , 

itAUV_v_temp, 

SLAUV_q_t  emp , 

6tAUV_y_dot_temp , 

itAUV_theta_dot_temp, 

iAlT'v'_del  ta_p  lane  s_t  emp, 

tA'J\'_s  tbd_rpm_temp , 

tAirv'_stern_vertical_temp, 

itAUV_stern_lateral_temp, 

tAU\'_ST1000_range_temp, 

StAUV_£T1000_strength_temp, 

tA'JV_ST7  2  5_range_t  em.p , 

&AUV_ST725_strength_temp) ; 


i.AUV_z_temp, 
&AUV_ps  i_temp , 
&AUV_w_temp, 
iAUV_r_temp, 
&AlJV_z_dot_temp , 
itAUV_psi_dot_temp , 


,aD^e?    oarsec    =: 


X 

y 

z 

pni 

v 
w 

p 

q 

r 

x_dot 

y_dct 

z_dot 

phi_dot 

theta_dot 

psi_dot 

del ta_rudder 

del ta_planes 

port_rpm 

stbd_rpm 

AUV_bow_vertical 

AUV_s  tern_ver t  i  ca 1 

AUV_bow_l a  t  e  r  a 1 

AUV_stern_lateral 

AUV_ST1000_bearing 

AUV_ST1000_range 

AUV_ST1000_strength 

AUV_ST7  2  5_bea  r i  ng 

AUV_ST7  25_range 


34) 


transfer  was  OK  so  keep  new  values  •/ 


=  (AU'v_time_temp)  ; 

=  (AU\'_x_temp)  ; 

=  (Al."/_y_temp)  ; 

=  (AU\'_2_temp)  ; 

=  (AUV_phi_temp) ; 

=  ( A'J^v_t h e t  a_t emp )  ; 

=  ( AlJ\/'_p  s  1  _t  emp  )  ; 

=  (AU'v_u_temp) 

=  (AU\'_v_temp) 

=  (Airv'_w_temp} 

=  {AUV_p_tempj 

=  {AU\'_q_temp) 

=  (AUV_r_temp) 

=  (AUV_x_dot_temp) 

=  (AUV_y_dot_temp) 

(AU\'_z_dot_temp) 
=  (AUV_phi_dot_temp) ; 

=  (AlPv'_theta_dot_temp)  ; 

=  (AUV_psi_dot_temp) ; 

=  (AUV_delta_rudder_temp) ; 

=  (AUV_delta_planes_temp) ; 

=  (AUV_port_rpm_temp) ; 

=  (AUV_stbd_rpm_temp) ; 

=  (AUV_bow_vertical_temp) ; 

=  (AUV_stern_vertical_teinp)  ; 

=  (AUV_bow_lateral_temp) ; 

=  (AUV_stern_lateral_temp) ; 

=nonnalize(AUV_ST1000_bearing_temp) ; 

(AUV_ST1000_range_temp) ; 

(AUV_ST1000_strength_temp) ; 
=nonnalize (AUV_ST725_bearing_temp) ; 

(AUV_ST72  5_range_temp) ; 


49 


AUv'_£T"725_strength   =         ( AUV_ST725_strength_teinpj  ; 

else  if  (  (variai:.les_parsed  !=  34)  &&  (variables_parsed  !=  -1)) 
{ 

if  (DISPLAYSCREEN) 
printf  ("\nGarble  problem  in  buf f er_received  !!!  variables  parsed  =  %d\n%s\n", 
variables_parsed,  buf f er_received) ; 
answer  =  getchar  ( ) ;  /*  pause  */ 
TRACE  =  TRUE; 
} 

if  iLOCATIONL.A&  ==  C)  speed  =  u;  /*  virtual  world  speed,  not  flow  sensor  */ 

li     (TRACE  Ui.    DISPLAYSCREEN) 

printf  ("\nfrom  virtual  world  buf f er_received : \n%s " ,  buf f er_received) ; 

if  iTR'JE  S.i  DISPLAYSCREEN) 

printf  ("\nfrorri  virtual  world  state  variables:'); 
printf  CNn  %s  t=%5.3f  x=%5.3f  y=%5.3f  z=%5.3f  ", 

keyword,  t, 

X ,  y  ,  2  )  ; 

printf  ("phi=%5.3f  theta=:%5.3f  psi  =  %5.3f  ", 

phi ,  theta ,  psi ) ; 

printf  '"u=%5.3f  v=%5.3f  w=%5.3f  p=%5.3f  q=%5.3f  r=%5.3f  ", 

u  ,  v ,  w , 

p ,  q ,  r )  ; 

printf  ( "x_dot=%5. 3f  y_dot=%5.3f  z_dot=%5.3f  ", 

x_dot,  y_dot ,  z_dot i ; 

print:  ( "phi_dct=%5 . 3f  theta_dot=%5 . 3f  psi_dot=%5 . 3f  ", 

phi_dot,  theta_dot,  psi_dot); 

printf  ( "delta_rudder=%5 . 3f  delta_planes=%5 . 3f  ", 

del ta_rudder ,        del ta_planes ) ; 
printf  I  "pcrt_rpin=%5  .  3f  stbd_rpm=%5  .  3f  ", 

port_rpiTi,  stbd_rpm)  ; 

printf    ( "bow_vertical=%5 . 3f    stern_vertical=%5 . 3f    ", 

AUV_bow_vertical ,     AUV_stern_vertical ) ; 
printf  ( "bow_lateral  =  %5  .  3f  stern_lateral:;%5  .  3f  " , 

AUV_bow_iateral,  AUV'_stern_lateral )  ; 

printf     fSTlOC'O    b/r/s    %5.3f    %5.3f    %5.3f,    ST725    b/r/s    %5.3f    %5.3f    %5.3f , 

AU\'_ST1000_bearing,   AU\''_ST1000_range,    AUV_ST100  0_strength, 

A'J\'_ST72  5_bearing,    AU\'_ST7  2  5_range,    AUV_ST7  25_strength)  ; 
printf  (",  [current  time  %d  %d  %d]  \n", 

systeiTi_tmp->tiTi_hour,    system_tinp->tm_iTiin,    system_tnip->tm_sec)  ; 
I 

/■•  keep   all  telemetry  variables  in  degrees  */ 

pr.i         =  normali2e2  (phi   ); 

thera        =  ncrmalizeT  (theta); 

ps.  =  normalize  (psi   i; 

pni_dct      =  normal ize2  (phi_dot); 

theLa_dot    =  normali2e2  (theta_dot); 

psi_dot      =  normalize2  (psi_dot); 

p  =  normal ize2  (p) ; 

q  =  normalize2  (q) ; 

r  =  norma lize2  (r ) ; 

delta_rudder  =  normalize2  (delta_rudder) ; 

delta_planes  =  normalize2  (delta_planes) ; 

if  (TRACE  &4.  LOCATIONLAB  &&  DISPLAYSCREEN) 
\ 

printf  (• \n'); 

) 

if  (auvdatafile  !=  NULL)  /•  output  data  to  telemetry  file        •/ 

i  /*  note  that  unmodified  stream  is  saved  */ 

if  (buffer_size  ==  0)  /*  nothing  was  received,  send  auv  state  */ 
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fprintf  (auvdataf ile,  •%s",  buffer); 
else  /■•  feedback  was  received,  send  uvw_state  */ 

fprintf  (auvdataf ile,  •%s',  buf fer_received) ; 

if  fTFJ^.-E  &6,  DISPLAYSCREENi 

printf (• [printed  to  %s  telemetry  file]\n",  AU\T)ATAFILENAME) ; 

/*  only  send/print  out  every  10th  telemetry  entry  to  tactical  level        */ 
due  to  serial  port  bandwidth  limitations  :-(  */ 

/*  rede  this  wiien  clock  logic  is  redone  *••♦***•••*•*•«***»•**•••*•**••*«»***••/ 

:f  (  'int  ■'  (  (t-0.05)  *10.0)  ==  (int)  (t  +  0.05)  'lO) 

if  (TACTICAL  ii  TRACE  &i  DISPLAYSCREEN) 

printf  ('(sending  data  to  tactical  level]\n"); 

*if  aef ined  (sgi ) 
«e.3t 

/•  writeln  (serialpath,  buffer,  255);    ««<<««<   */ 

if  (TACTICAL  ==  TRUE)  write    (serialpath,  buffer,  255); 
«endi  f 

if  (TACTICAL  &i,  TRACE  &&  DISPLAYSCREEN) 

printf  (" [write  buffer  to  tactical  level  serialpath  OK)\n'); 

if   a^vtextfile  !=  NULL)  /*  output  data  to  .auv  text  file     '/ 

if  (TRACE  i.4.  DISPLAYSCREEN, 

printf  ('[sending  data  to  .auv  text  file]\n'); 

fprintf  (auvtextf ile,  •%s",  buffer); 

if  (buffer_Size  !=  0)  /'  feedback  was  received,  also  send  uvw_state  */ 
fprinrf  lauvtextf ile,  •%e",  buffer_received) ; 

If  'TRACE  6,6,  DISPLAYSCREENI 

prinif  ;"lfprintf  tc  .auv  text  file  OK]\n'); 
I 

tfclen^itt ry_records_saved  -->■; 

if    ! i (buf fer_index  -1)  %  FILEBUFFERSIZE)  ==  0)  buffer_index  =  0; 
else  buffer_index  ++; 

/'  nt-ec  to  copy  Duffer  to  buffer_array  if  caching  telemetry  <«««««<   •/ 

if  (TRACE  6,6,  E>ISPLAYSCREEN)  printf  (  '  [buf  f  er_index  =  %d]\n',  buf  f er_index)  ; 

/* */ 

/*  test  code  to  send  data  from  file  serial. d  from  wr2tl.c  */ 

/*  read  characters  from  file,  echo  characters  to  screen,  */ 

/*    send  characters  to  serialpath  /tl  device  */ 

/*     while  ((c[0]  =  getc (serialtestf ile) )  !=  EOF) 

( 

putc    (c [0] , stdout) ; 
writeln  (serialpath,  c,  1); 

) 
•/ 
/* */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('[finish  record_data  {)]\n'); 

return; 
) 
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douDlt  rolI_angle  ()     /*  Return  roll  angle  in  RADIANS  •/ 

int    val ; 

double  angle  =  phi;     /*  previous  (or  virtual  world)  value  •/ 

if  (TRACE  &&,  DISPLAYSCREEN)  printf  ("(start   roll_angle  ()]\n"); 

if  (N07_YET_REIMPLEMENTED) 
I 

val  =  add  (ROLL_ANGLE_CH)  ; 
/•   angle  =  ((516.578  -  val ; /5 .7572 ) /57 . 295779 ;   convert  to  radians  */ 

angles  (-.1737*val  +  . 1737«roll_0 ) /57 .295779; 
) 

angle  =  normalize2  (angle); 

li     (TRACE  6,4,  DISPLAYSCREEN) 

pimtf    {"[finish    roll_angle    ()    returns   %5.3f]\n",    angle); 

re"  LiTTi    ( a;"!gle  i  ; 
1 

douDle  L:tc.h_angle  ()     /*  Return  pitch  angle  in  RADIANS  */ 

in:    val; 

acuble  angle  =  theta;    /■•  previous  (or  virtual  world)  value  */ 

■f  ■  TRACE  Sri,  DISPLAYSCREEN)  printf  ("[start   pitch_angle  ()]\n"); 

if  'NOT_YET_REIMPLEKENTED) 

val  =  add  (PITCH_ANGLE_CH)  ; 
/*    angle  =  ((520.153  -  val ) /8 . 340  ^'57 . 295779;   convert  to  radians  */ 
angle  ^    -  (  (- .  1 199'val  +  .  1199'*pi  tch_0) /57  .  295779  )  ; 


angle  =  norinalize2  (angle); 

if  fTPJ^CE  4,6,  DISPLAYSCREEN) 

printf  ("[finish  pitch_angle  !)  returns  %5.3f]\n",  angle); 

return  (angle, ; 
1 

double  heading  () 

/'  Return  heading  angle  with  respect  to  local  magnetic  north  in  radians 
from      directional  gyro  */ 
{ 

unr^igned  shorr  dg_b:  t ; 
double         angle; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('(start   heading  ()]\n'); 

if       (LOCATIONLAB  &4c  (DEADRECKON  ==  FALSE)) 
( 

angle  =  psi; 
) 
else  if  (LOCATIONLAB  &&  (DEADRECKON  ==  TRUE)) 
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angle   =   psi_cominand; 

ei  se 
1 
if  ;NOT_YET_P,EIMPLEMENTED) 
{ 

dg_bit  =  Read_PortAB(MFI_BASE) ; 
dg_tic  4,=  0x2 FFF; 


) 


) 


anglf  =  (3.8350e-4)  "  dg_bi t ; 
) 

angle  =  normalize  (angle); 

if  (TRACE  ii  DISPLAYSCREEN) 

printf  (" (finish  heading  ()  returns  %5.3f]\n',  angle); 

return  (angle) ; 


double  roll_rate_g:>'ro  ()     /*  Return  roll  rate  in  RADIANS/SEC  *, 

{ 

int    val ; 

dcuDle  rate  =  p;   /*  previous  (or  virtual  world)  value  */ 

it    'TRACE  &s<  DISPLAYSCREEN,  printf  C  [Start   roll_rate_gyro  ()]\n'); 

*-  f  i-r-:  .ned  '  sgi ; 
♦lelse 

val   =  add  !ROLL_RATE_CHi  ; 

rare  =  ! roll_rate_0/3 . 2113  -  . 31062*val ) /57 . 295779 ; 
*end^ : 

rate   =   norinalize2    (rate); 

:f  .'TRACE  £<(.  DISPLAYSCREEN) 

prmtf  ("[finish  rcl  l_rate_g>'ro  ()  returns  %5.3fl\n",  rate); 

return  (rate; ; 

/*»*««*«»*♦«*««»»*»•**«♦****«*«**««*««*•»«»***«««*«*«***««*«****»««««*««*»****♦ 

dour..e  pi  t  ch_rate_gyro  ()     /•  Return  pitch  rate  in  RADIANS/SEC  *, 

i 

int    val   =0; 

double  rate  =  q;   /*  previous  (or  virtual  world)  value  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("(start  pitch_rate_gyro  ()]\n"); 

«if  defined  (sgi) 
#else 

val    =  add  ( PITCH_RATE_CH)  ; 

rate   =  (pitch_rate_0/13 . 69399  -  . 0730001*val ) /57 . 295779; 
♦endif 

rate  =  nonTialize2  (rate); 

if  (TRACE  4.S,  DISPLAYSCREEN) 

printf  ('[finish  pitch_rate_gyro  ()  returns  %5.3f]\n",  rate); 

return  (rate) ; 
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douh.  :e  yaw_rate_gyro  ()     /*  Return  yaw  rate  in  RADIANS/SEC  */ 

int    val   =  0; 

double  race  =  r;   /*  previous  (or  virtual  world)  value  •/ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"(start  yaw_rate_gyro  ()]\n'); 

#if  defined  (sgi) 
#else 

val   =  add  (YAW_RATE_CH)  ; 

rate  =  (yaw_rate_0/13 . 653216  -  . 0732362*val ) /57 . 295779 ; 

♦  endi  f 

rate   =   nornialize2    (rate)  ; 

if  (TRACE  iS.  DISPLAYSCREEN) 

prjntf    ("[finish  yaw_rate_gyro    ()    returns   %5.3f]\n",    rate) ; 

return    ( rate i ; 


:**■*«»*****«***«***««**«******«*«**«««*»**«***«*«*♦***/ 


dcjr.le   ?tbc_iTiOtor_rpm    ()  /*    Reads    rpm   from   RIGHT_MOTOR 

int  pulse; 

double   local_stbd_rpiTi; 

::     (TRA'.'E   ii.    L'ISPLAYSCREEN)    printf    ("[start      stbd_motor_rprn    ()]\n"); 

pulse  =    add  (RIGHT_MOTOR_RPM)  ; 

local_stbd_rpm   =    1.244*pulse    -    8.4792; 

if  (TRACE  US.    DISPLAYSCREEN) 

printf    ("[finish  stbd_iTiotor_rpiri    ()    returns   %5.3f]\n",    stbd_rpni)  ; 

return    : local_stbd_rpm) ; 


/•»*»«▼»*»*»»««    »irw»»*»1(ir««»«*Tt«**' 


r******lt«*«ir*lt«**«*««««****«««***« 


double  port_motor_rpiT:    (,  /*   Reads   rpm   from  LEFT_MOTOR  */ 

I 

int  pulse; 

double  local_port_rpm; 

if  (TRACE  iir  DISPLAYSCREEN)  printf  ("[start   port_motor_rpm  ()]\n"); 

pulse  =    add  {LEFT_MOTOR_RPM)  ; 

lr-,ca2_porc_rpm   =    1.244*pulse   -    8.4792; 

if     (TRACE   &6r    DISPLAYSCREEN) 

printf    ("[finish  port_motor_rpm    ()    returns   %5.3f]\n",    local_port_rpm) ; 

return    (local_port_rpm) ; 

1 

/♦««»••««»«*»•»««»»**••«*•*«*»***•»*•**•••*•***********»***«****»«««**«*«•**«*«/ 

double  get_speed  ()   /•  Filter  the  speed  signal  */ 

{ 

int    index; 

double  avg_speed  =  0.0; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf    ("[start     get_speed    {),    LCX:ATIONLAB=%d] \n" ,    LOCATIONLAB) ; 
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if  (LOCATIONLAB) 

if  (TRACE  iS.  DISPLAYSCREENy 

print f     ("(finish   gec_speed    ()    returns    " ) ; 
avg_speed   =    (speed_per_rpm   '     (port_rpiri   +    stbd_rpin)    /    2.0); 
if    (TRACE   &£.    DISPLAYSCREEN) 

printf    (nS.BfJXn",    avg_speed); 

return  iavg_speed) ; 
} 

else 
I 

speed_array [pointer]  =  adc2(l,0); 
for  (index=0 ; index<ll ; ++index) 
( 

avg_speed  +=  speed_array [ index] ; 
} 

avg_speed  =  avg_speed/ 1 1 ; 
avg_speed  =  avg_speed  *  0.003635; 
:f  (avg_speed  --  0.78) 
( 

if  (t  <  10 .0) 
{ 

avg_speed  =  speed  +  0  .  000045' (rpm-150 ) ; 
if  (avg_speed  >  (  rpni*0  .  002  )  )  avg_speed  =  rpm*  0.002; 
) 

else 
( 

avg_speed  =  1.0; 
) 
:  f  i  t  <  2.0)  avg_speed  -    0.0; 

pointer  =  pointer  ■>■    1; 
it    (pointer  >  10)  pointer  =  0; 
I 

if  (TRACE  5.6.  DISPLAYSCREEN) 

printf  {• (finish  get_speed  (^  returns  %5.3f]\n',  avg_speed); 

returr,  ;  avg_spced  ■  ; 


void  get_init_avg  () 
i 

m:  index,  rng_sum; 

If   TRACE  iSr  DISPLAYSCREEN!  printf  ("(start   get_init_avg  ()]\n"); 

rng_surri     =  0 ; 
range_index  =  0; 

for (index  =  0;  index  <  AVG_PTS;  ++ index) 
{ 

via0[ORB_IRB]  =  (S0NAR_SW1  &  S0NAR_SW3 )  I  S0NAR_TRIG2; 

viaO[ORB_IRB]  =  S0NAR_SW1  &  S0NAR_SW3 ; 

tsleep (5! ; 

range  =  adc2 (3,0) ; 

rng_sum  +=  range; 
range_array [index]  =  range; 
++range_index; 

) 

avg_rng  =  (rng_sum/AVG_PTS)  *  1.0; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('[finish  get_init_avg  ())\n"); 
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return; 


:*»**«**»«»**««*»♦**»«**■**«♦***«««*******«**«***»*« 


*»»»«»»»»»»«»»/ 


vcid  get_avg_rng  () 

inc  index,  UPDATE_AVG,  int_rng_sum; 

if  (TRACE  &Sr  DISPLAYSCREEN)  printf  ("[start  get_avg_rng  ()]\n'); 

UPDATE_AVG   =  0  ; 
int_rng_suin  =  0; 

if  ((( float ! range  >  avg_rng  )  M 

!fabs( (float ) range  -  avg_rng)  <=  MAX_RNG_DIFF)  II 
(bad_rng  >=  MAX_BAD_PTS) ) 
( 

range_array [range_index]  =  range; 
*-i-rang6_index; 
UPDATE_AVG  =  1; 
iflbad_rng  >  MAX_BAD_PTS) 

+  -fbad_updates ; 

if ;bad_updates  >=  MIN_NG_PTS) 
( 

bad_rng  =  0; 

i 

else 

i 

+*bad_rng; 

if  [i;prATE_AVG) 
{ 

for (index  =  range_index  -  AVG_PTS ;  index  <=  range_index;  ++index) 
( 

int_rng_suiT.  +=  range_array  [  index]  ; 

avg_rng  =  inr_rng_suiTi/AVG_PTS  •  1.0; 
i 
if  .'TPACE  i,i.    DISPLAYSCREEN)  printf  ("[finish  get_avg_rng  ()]\n"); 

return; 
) 

/  * 

Hardware  control  changes  *FOLLOWING»  hardware  upgrade  1993: 

Telemetry  to  tactical  level:  serial  port  /Tl  via  driver  /TT 

Orders  froiTi  tactical  level:  parallel  port  /P  via  MFI  register  A 

Sonar:  interface  card  device  driver  /T3 

•/ 

void  open_device_paths  () 
{ 

if  (TRACE  s<&  DISPLAYSCREEN)  printf  ("[start  open_device_paths  ()]\n"); 

#if  defined  (sgi) 


56 


*  trlcJici  /tl  serial  port  #1  or  /tt  (high  baud  rate  driver  for  /tl)      •/ 
serialpath  =  ope,-.  C/tl",  S_IREAD  +  S_IWRITE)  ;        /*  get  path  number  •/ 

/tt  IS  device  for  high  baud  rate  /tl  serial  port     */ 
:f  iser^alpath  <=    0) 
i 

print f  ( "open_device_paths  ():   unable  to  open  serialpath  /tl .   ' ) ; 
print f  ( "Exit . Nn" ) ; 
exit  (-1 ) ; 
) 
if  (TRACE  &&  DISPLAYSCREEN) 

pr^ntf  ("[serialpath  /tl  (normal  baud  rate)  open,  path  number  =  %d]\n", 

serialpath) ; 

If  I  SONAR INSTALLED  ==  TRUE) 
I 

sonarpath   =  open  ('/t3",  S_IREAD  +  S_IWRITE) ;      /*  get  path  number  */ 
/*  /t3  is  device  for  sonar  interface  card  •/ 

if  (sonarpath  <=  0) 
1 

printf  ( •open_device_paths  ():   unable  to  open  sonarpath  /t3.   '); 
printf  ( "Exit . \n" ) ; 
exit  ( -1  )  ; 

if  1  TRACE  &i  DISPLAYSCREEN) 

printf  ("[sonarpath   /t3  open,  path  number  =  %d]\n",  sonarpath); 

tty_mode  i sonarpath , 1 ) ;    /*  initialize  sonar  values  */ 

else    If  (TRACE  &,&  DISPLAYSCREEN) 

printf  ("[sonarpath   /t3  ignored,  SONAR INSTALLED  ==  FALSE] \n"); 

/■*  otner  paths:   effectors,  depth_sonar,  etc.  •••«•»••«•«••»•«•«*••««*•••»/ 

*^nd-f 

i:     (TRACE   &&    L'ISPLAYSCREEN)    printf     ("[finish   open_device_paths    ()]\n'); 

return; 

vcid  clo?e_device_p3ths  () 

if  (TRACE  ii  DISPLAYSCREEN)  printf  ("[start   close_device_paths  ()]\n"); 

if  'serialpath  >  0)  close  (serialpath);   /*  test  for  open  before  closing  •/ 

else  if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[serialpath  was  not  open!]\n'); 

if  (SONAR INSTALLED) 

if  (sonarpath  >  0)  close  (sonarpath); 

else  if  (TRACE  &&  DISPLAYSCREEN)  printf C [sonarpath  was  not  open!)\n'); 
) 

/*  other  paths:   effectors,  depth_sonar,  etc.  «««««««««««««  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('[finish  close_device_paths  ()]\n"); 

return; 

) 

/ ,».....,.....................*.,.........................,.,......../ 

void   read_parallel_port  ()    /*  loop  and  display  8  bit  data  from  port  A  */ 
f 


57 


static  char  next_char,  last_char; 

static  char  current_command  [256]; 

static  int  index; 

unsigned  char  temp; 

if  (TRACE  l,ic    DIEPLAYSCREEN) 
( 

printf  ('(start   read_parallel_port  (),  "); 

if  (PARALLELPORTTRACE)  printf  ( ' PARALLELPORTTRACE  is  ON]\n"); 

else  printf  ("PARALLELPORTTRACE  is  OFF]\n'); 

) 
/*  see  initialize_adcs  ()  for  Init_PortA  &  B  code  */ 

4iif  defined  (sgi) 

/•  Read  PortA  parallel  port  character  by  character  for  tactical  orders  */ 

/"  reference:   Walt  Landaker's  mfi_a3.c  in  directory  /hO/AUV  and  */ 

/*  page  3-12  of  Motorola  6800  Series  Manual  for  6821  PIA  */ 

Programmable  Interface  Adapter.  */ 


/• 


/''   Wirninqi   You  may  have  to  reset  both  computers  to  get  the  parallel  */ 

"  porr  tc  read  i  write  properly.   Additionally,  •/ 

or.  tne  36 1  you  can  run  PORTFIX  to  reset  parallel  port  LPTl  :  */ 

terr,"  =  Read_PortA  ((struct  MF1_PIA  *)  MFI_BASE)  ;      Z*  should  clear  busy!  '/ 

index  =  0; 

■■'  read  port  status  (note  sta  not  stb)  */ 
PcrtAFlag  =  ck_?ta  ((struct  MFI_PIA  *)  MFI_BA£E) ; 

if  'PARALLELPORTTRACE  &&  DISPLAYSCREEN) 

printf  ("\n     (time  %5.2f  read_parallel_port  ()  resumed]",  t) ; 

while  fPortAFlag  &£<  0x80)         /*  see  loop  break  for  alternate  exit  */ 

I 

I*   Note  that  ck_stb  is  used  in  mfi_a3  but  ck_sta  makes  more  sense  */ 
PortAFiag  =  ck_sta  ((struct  MFI_PIA  •)  MFI_BASE);  /*  read  port  status  */ 

last_cnar  =  next_char;       /*  read  char  and  reset  busy  */ 
next_char  =  Read_PortA ' (struct  MFI_PIA  *)  MFI_BASE) ; 

if  ('PortAFiag  --    0x24^  &£,  (last_char  ==  next_char))  break; 

/•   if  next_char  changed  then  flag  may  be  messed  up,  read  anyway  •/ 

/•  check  for  ptr  strobe  */ 

/*  break  =>  no  character  waiting  */ 

/•  control  passes  outside  while  loop  */ 

else  if  (next_char  ==  13)     /*  CR  indicates  end  of  line  */ 


current_command  [index]  =  13 

current_comrr.and  I  index -^1]  =  10 

current_command  [index+2]  =   0 
index  =  0; 


/*  CR    /n  •/ 

/•  LF    extra,  not  needed      */ 
/*  end  of  string  delimiter      */ 


if  (auvtextf ile! =NULL) 
{ 

fprintf  (auvtextf ile,  ■%s",  current_command) ; 

f flush   (auvtextf ile) ; 
} 

if  (DISPLAYSCREEN) 
{ 

printf  (•\n\n»>  time  %5.2f  tactical  message  «<\n',  t); 

printf   (•%s",  current_command) ; 

printf   CXn"); 
} 
/********  insert  command  processing  logic  here  <«««««««« 
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else  if  !nexc_char  !=  10)  /•  LF  ignored,  others  appended    */ 

( 

current_comniand    [index]    =   nexc_char; 

index    ++ ; 

if  (PARALLELPORTTRACE  S.I.    DISPLAYSCREEN) 

( 

/•  print  character  to  screen  */ 

printf  ("Xn  %c  =  %2x,  PortAFlag  after  reading  =  %4x] 
next_char,  next_char,  PortAFlag); 
) 
) 
I  /*  end  while  loop  to  read  characters  from  port  */ 

if  (PARALLELPORTTRACE  i.S.  DISPLAYSCREEN) 
i 

printf ("     [%c  =  %2x,  PortAFlag  =  %2x,  exiting  read_parallel_port  ()]', 
next_char,  next_char,  PortAFlag); 
} 
«endi  f 

if  (TRACE  &i  DISPLAYSCREEN;  printf  ("[finish  read_parallel_port  ()]\n'); 

retijrr. ; 

)   /•  er.d  reaa_parallei_port  */ 

,'  :i:c  arrangemen:  in  AL^'  ■'PRIOR'  to  hardware  upgrade  1993: 

I  <--  Direct 
I     Gyre 


1 
1 

1 

1 

1  1 
!   1 

Reg 

MFI 

A  and  B 

1 
1 

adc-2 
(0-15 

1 

1  <--  Sonars 
1 

1   ada-1 
1   (  0  - 

dac 

3  ) 

I   1 
1   1 

ada- 
(  0 

-1  adc       1 
-  15  )       1 

1 
1 

dac-2b 
(  0  -  7  ) 

1  <--  Planes 
1 

Ka;n  motors  -->  I   ada-1  dac    I   I       ada-1  adc        I  <--  Gyros 


This  file  contains  the  functions  which  address  the  A/D  D/A  cards 
directly  in  terms  of  voltages.  •/ 

*  dacl(s,ch)  --  writes  signal  's'  to  ada-1  dac  channel  'ch' 

•  (allowable  channels  0-3) 

void  dacl (s, ch) 

int  s,ch; 

( 
if  (NOT_YET_REIMPLEMENTED) 
{ 

ch  =  ch  «  2;  /*  offset  for  G-96  addressing    */ 
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dacl_alch]  =  s  >>  2;  /*  write  upper  8  bits  to  MSB     */ 

dacl_alch  *  DAC_LSB_OFFSET]  =  s  «  6;    /*  write  lower  2  bits  B3,B2      */ 


) 


re" u  r n ; 
)  /•  dacl  •/ 

/»«*»«»«*«*«•*****»**«*********♦«**♦*»**»«*******«******«*************«*«***** 

*  dac2b(s,ch)  --  writes  signal  's'  to  dac2b  dac  channel  'ch' 

*  (allowable  channels  0-15) 

void  dac2b(s, ch ) 
i  n  t  s  ,  c  h  ; 

:f  (NOT_YET_RE IMPLEMENTED) 
{ 

ch  =  ch  '■<  2;  /*  offset  for  G-96  addressing   */ 

dac2L_aich)  =  s  >>  2;  /■*  write  upper  8  bits  to  MSB    •/ 

•ja  ;:i:_o:  ..:!■:  -^  [.AC_LS&_OFFSETj  =  s  <v  6;     /*  write  lower  2  bits  B3,B2     •/ 
i 

return ; 
»  /'  dac2i:j  */ 

adcln'  --  reads  ada-1  ado  channel  'n'  (channels  0-15) 

int  adc: (n; 
i  n  t  n  ; 

int  val ; 

:f  iNCT_YET_REIMFLEMENTED) 
{ 

adcl_a [ADCl_CMD_REGi  =  n ; 

while  (adc:_a :ADC:_STArj£_REG;  >  20);     /*  wait  for  data  */ 

val  =  adcl_a 'ADC:_MSBj  <<"  2; 

val  *=  adc:_a ;ADC1_LSE]  »  6; 

return  tval ; ; 

)  ei  ?e  return  '  0 '  ; 

^   '    d  ^  C  i    / 

*  adc2;n,g);  --  Reads  adc-2  channel  'n'  (0-15) 

with  gain  'g'  (0  to  F  =>  0  -  1024) 
* ........»..........»...........-................,,..,.........../ 

int  adc2  \r.,gi 

int  n , a ; 

( 

int  val; 

If  (NOT_YET_REIMPLEMENTED) 
( 

adc2_a (ADC2_CH_GAIN]  =  (n  «  4)  I  g;      /*  set  c&g,  start  conv  */ 
while(  (adc2_a  |ADC2_STATUS_REG)  £,  0x7)  !=  0);   /*  wait  for  ready  */ 

/*  This  adc  uses  0  -  4095  to  represent  full  scale  input,  in  order 
to  write  to  the  dac  (which  uses  0  -1023  for  full  scale)  you 
must  divide  val  by  4  or  shift  right  by  2.  Use  the  next  line  to 
get  full  resolution, 
val  =  adc2_a [ADC2_DATA) ; 
The  next  line  is  used  for  testing  purposes  only 

val  =  adc2_alADC2_DATA]  »  2;   */ 
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val  =  adc2_a[ADC2_DATA] ; 
vai  =  vai  t,    OxOFFF; 

return  val ; ; 
)  else    recurr,  10}  ; 
•'  adc2  •/ 

/■• 

The  program  code  for  the  Multi-Function  Interface  originated  from  'mfi.c' 

Routines  include  Init_PortA,  Init_PortB,  Read_PortA,  Read_PortB,  Read_PortAB 

Excerpt  of  'infi.c'  comments  follows: 

Program  example  for  the  Multi-Function-Interface  (MFI ) 
This  example  uses  the  6821  PIA  on  the  MFI  board 
General  purpose  functions  are  provided  to  initialize  the  PIA 
and  read/write  data  to  the  ports 

MFI  P2  connector  definitions  are  provided  by  the  GESMFI-1  data 
sheet  available  from  GESPAC,  Inc. 

6821  device  specifics  are  covered  in  the  6-bit  microprocessor 
&  peripheral  data  book  from  Motorola  Inc. 

3/1/91        J.  Rawlins      RealTime  Software  Consulting 

Ini t_PortA (base,  dir)  --  Initialize  Port  A  of  MFI 

dir:  1  =  output  port,  0  =  input  port 

void  In:t_PGrtA  (base,  dir) 

register  struct  MFI_PIA  'base;/'  base  address  of  MFI  board  on  G96  bus       */ 
mt  dir;  /*  direction:  1  =  output  port,  0  =  input  port  */ 

register  snort  temp; 


/•  save  contents  of  control  reg .  (no-op)  */ 

/•  select:   b2  =  0  data  direction  reg.  */ 

/•  set  pcrtA:  0  =  input  */ 

/•  select:  access  data  reg.s  (b2=l)  */ 

/•  b5=l ,b4=G,b3=0 (read  w/cal  restore)  */ 
Init_PortA  */ 


temp  =  Dase->cra 
base->cra  =  0x0  0 
base->pra  =  0x00 
base->cra  =  0x24 


/**«**«**»»«♦********««•**♦*****«*****«********«*****«**•****♦***♦***«««*«**** 

Init_PortB(base,  dir)  --  Initialize  Port  B  of  MFI 
*  dir:  1  =  output  port,  0  =  input  port 

•••»»«»•»«»*»»«»»*♦««•»•*»•»«»*«»»*«««*»»»»»»♦»«»»•«»»*««•«««««««♦««*»»•«»♦«/ 

void  Init_PortB  (base,  dir) 

register  struct  MFI_PIA  *base;   /•  base  address  of  MFI  board  on  G96  bus       •/ 
int  dir;  /•  direction:  1  =  output  base,  0  =  input  base  */ 

{ 

register  short  temp; 

temp  =  (base->crb  &  OxOOFF) ;   /*  get  current  value  of  control  A  •/ 

temp  &=  -4;  /*  clear  bit  #2  so  we  can  access  ddra  */ 
base->crb  =  temp; 

if  (  dir  )  /*  make  port  B  all  outputs  •/ 

base->prb  =  OxOOFF; 

else  /•  port  B  is  all  inputs  */ 
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/*  set  bit  #2  to  access  data  registers   •/ 


base->prb  =  0x0000; 
t  emp  I =  4 ; 
base->crb  =  temp; 
/'  In-t_PortB  '/ 

/«**»«««■«**»*««*****««*«**•**♦*«««**«*«*«*«*******♦**«**««**«***«*******«»•»*«* 
*   Read_PortA   (base)  --  returns  8  bit  value  from  port  A 


«««v««****1t*«»*****»«: 


r****************i 


r*»»««»»»»»*«»*»»«*«»«** 


unsigned  char  Read_PortA  (base) 

rr-q:=ter  struct  MFI_PIA  •base; 
i 

register  unsigned  short  temp; 

temp  =  base->pra; 

return (temp  &  OxOOFF) ; 


/*  base  address  of  MFI 


/*  read  data  reg. should  reset  busy  */ 
/*  return  data  to  calling  program  •/ 


:«****«**«««•**««»*»***: 


«»»»*««*»•»**«»•«**»' 


R»rad_PcrtB      (base)    --    return?   8   bit   value   from  port   B 


*«***! 


:****«**««**«*«*»****«**• 


r  *  *  *   *  * 


•j!'.?:qned   char   Read_PortB    (base) 
->^.-.^.    struct   MF:_PIA    *basfe; 

register  unsigned  short  temp; 
tem,p  =  base->prb; 

return (temp  &  OxOOFF); 


/«  base  address  of  MFI  •/ 


'   Reac_PortAB  (base)  --  return  a  16  bit  value  from  ports 

A  and  B  combined  then  mas)<  off 

the  15  th  and  16  th  bits. 
*  Ncte:  PIA  PA0-PA7  is  the  LSB  and  PB0-PB7  the  MSB 

unsigned  short   Read_PortA&  (base) 

:eg-ottr:  struct  KF:_PIA  'base;       /'  base  address  of  MFI  •/ 

register  unsigned  short  hi, lo, temp; 

lo  =  (base->pra  &  OxOOFF);  /*  get  least  significant  byte  from  A  */ 

hi  =  (base->prb  &  OxOOFF);  /*  and  most   significant  byte  from  B  • / 

temp  =  ((hi  <<   8)  +  lo);  /*  shift  hi  into  upper  byte  of  word  */ 

return  (  temp  ;;  /•  return  data  */ 


/' 


r  **«♦««  : 


r«***»««**«***««**Tti 


r*«*«****«««»»**»««**««»««*»*l 


void  set_bsyA (base)   /*  sets  CB2  high  (for  busy  to  sending  port; 

register  struct  MFI_PIA  *base;   /•  base  address  of  MFI 

I 

register  short  temp; 

temp  =  (base->cra  &  OxFF) ;   /*  save  era  values 
base->cra  =  0x38;  /*  8  bit  1=   CR2   high 

base->cra  =  temp;  /*  restore  era  values 

) 

/•  sets  CB2  low  (for  -busy  to  sending  port)  */ 

void  rst_bsyA (base) 

register  struct  MFI_PIA  "base;   /*  base  address  of  MFI 

{ 

register  short  temp; 
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temp  =  (base->cra  &  OxFF) ;   /*  save  era  values  */ 

base->cra  =  0x30;  /•  8  bit  0=  CR2  low  */ 

base->cra  =  temp;  /*  restore  era  values  */ 

int  ck_sta  (base) 

register  struct  MFI_PIA  'base;   /*  base  address  of  MFI  */ 

{ 

register  unsigned  short  temp; 

temp  =  base->cra;  /*  save  era  values  •/ 

1  et  Lirn  ( temp;  ; 
) 


r 


«»»«*«••*«««««»*»»«»»»««.*»»««»»*«♦»««»♦««««*««»»«««♦»»*••***«»*»**»*«•«** 


void  center_sonar  () 
i 

int  direction, encoder_width; 

char  encode; 

if  (!  SONAR INSTALLED) 
( 

printf  ("[start/stop  center_sonar ,  SONAR INSTALLED  false] \n"); 

return ; 
) 

if  iTRACE  ii  DISPLAYSCREEN)  printf  ("(start   center_sonar  ()]\n*); 

encoder_width  =  0; 
direction  =  1; 

I"    set_step_si2e ( 'H' ) ;  */  /•  '1'  =  0.9,  '2'  =  1.8,  '4'  =  3.6  */ 

'*  Are  we  inside  the  Encoder  Sensor  ?  */ 

encode  =  query_scnar_l_repiy  ('M';;    /•  Test  Head  Direction  (No  Step)  */ 

If  iSONAPTRACE  4.&  DISPLAYSCREEN) 

printf  " centei_sonar :   encode  =  %c \n" , encode) ; 

if  ',  [encode  ==  't'j  II  (encode  ==  'T' )  ) 

while!  (encode  ==  't')  II  (encode  ==  'T')  ) 

encode  =  query _sonar_i_reply  ('+');  /•  Index  Sonar  '+'  direction  */ 
) 

/*  Outside  Encoder  Sensor  Now  */ 

direction  =  -1;  /*  Reverse  Sonar  Rotation  to  Establish  Encoder  Width  •/ 
) 

while (  (encode  ==  'f')  II  (encode  --    'F')  ) 
( 

if  (direction  ==  1 ) 
{ 

encode  =  query _sonar_l_reply  ('  +  ');  /•  Index  Sonar  '  +  '  direction  •/ 
if  (SONARTRACE  Sc&  DISPLAYSCREEN)  printf  ( •%c\n' .encode)  ; 
) 

else 
{ 

encode  =  query _sonar_l_reply  ('-');  /*  Index  Sonar  '-'  direction  */ 
) 
} 

/*  Found  Edge  of  Encoder  */ 

while!  (encode  ==  't')  II  (encode  ==  'T')  ) 

i 

encoder_width  =  encoder_width  +  1; 

i  f (direction  ==  1 ) 
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{ 

encode  =  query _sonar_l_reply  ('+');  /*  Index  Sonar  '+'  direction  •/ 
} 

else 
( 

encode  =  query _sonar_l_reply  ('-');  /'  Index  Sonar  '-'  direction  •/ 
} 
) 
if  iSONARTRACE  &S.  DISPLAYSCREEN) 

piintf  ( "cencer_sonar :   encoder  width  =  %d\n" , encoder_width) ; 

if  (TRACE  £<£.  DISPLAYSCREEN)  printf  {"[finish  center_sonar  ()l\n'); 

return; 
■  /'  end  center_?onar  f)  */ 
/»»»»»»»«»*«»*»*«*«»««»**«****««*»*****««*«***«**«*******«***«****«*******♦***«/ 

char   q-uery_sonar_l_repIy    (command_char ) 
cr.ai    comjT,and_cna r  ; 

'  rode  tested  i  taken  from  headtest.c  (prior  version  ahead. c)  */ 

.r.t  :r-de>:,  r. ,  n_b\-tes; 
c.'iir  rep  2y  ,  xx  i  2u  ]  ,  c  1 1  ]  ; 

if  (!  SONAR INSTALLED) 

printf  ("[start/stop  query_sonar_l_reply  (),  SONAR INSTALLED  false] \n"); 
reply  =  '  '  ; 
return  ( reply )  ; 
) 

it      TRACE  it  DISPLAYSCREEN".  printf  ("[start   query _sonar_l_reply  ()]\n"); 

'*  liKely  string  problems  here  <<<<<<<<<<<«<<<<««<<<<««««««««<<  •/ 

c  ■  ci  i  =  cominand_cnar  ; 

:f  ■:  SONARTPJiCE  i<£<  DISPLAYSCREEN) 

printf  ( "query _sonar_l_reply  :   command_char  =  %c\n",  coininand_char)  ; 
n    =  write  (sonarpath, c, 1 ) ;   /'  write  characters  to  sonarpath  /t3  device  "/ 

tsleep { iO) ; 

n_byte£  =  _as_rdy  (sonarpath); 

.:  'SONARTRACE  ii  (n_Dytes  >    1)    &&  DISPLAYSCREEN) 

printf    ; "query _sonar_l_reply :      lost    reply   data,    n_bytes   =   %d\n" ,n_bytes) ; 

if    (n_bytes   <=    0) 
{ 

printf ( ■query_sonar_l_reply:      bad   read,    n_bytes   =   %d\n" ,n_bytes) ; 
1 
else 

n  =  read  ( sonarpath, xx, n_bytes) ;   /•  n  unused??????  •/ 

if  (SONARTRACE  &&  (n_bytes  >  0 )  &6<  DISPLAYSCREEN) 
{ 

for  (index  =  0;  index  <  n_bytes;  index++) 
{ 

printf (•(%c  %2d  %2x]   ',  xx[index),  xx[index],  xx[index]); 
if  ( (index+1)  %  5  ==  0) 

printf ( "Xn" ) ;  /*  prevent  writing  off  screen  */ 
) 
printf  CXn")  ; 


64 


I 
} 
reply  =  xx[0] ; 

if  (TRACE  ici,  DISPLAYSCREEN) 

prantf  ("[finish  query _sonar_l_reply  ()  returns  %c]\n',  reply); 

return  ( reply) ; 
) 

/««♦♦*♦«»*****»**«♦*****«»»**»«*»«*««**«♦*«**«****«***«********«********«**♦***/ 

void  set_step_size  (step_code) 

char  step_code; 

unsigned  int  n; 
char         reply; 

if  (!  SONAR INSTALLED) 

printf  (• [start/stop  set_step_size  (),  SONAR  INSTALLED  false] \n'); 
return ; 

if  (TRACE  &i  DISPLAYSCREEN)  printf  C [start   set_step_size  ()]\n'); 

if  (SCNARTRACE  ii  DISPLAYSCREEN!  printf  ('step  code  =  %c\n" , step_code) ; 
write  (sonarpath, step_code, 1 ) ; 
tsieep  (Ii;   /*  2S6ths  of  a  second  */ 

n  =  re  =  d  'scr.arpath,  reply,  1);    /•  n  unused??????  •/ 

if  iSONARTF^CE  4<&  DISPLAYSCREEN)  printf;  "step  =  %c\n' ,  reply )  ; 

if  (TRACE  ii  DISPLAYSCREEN)  printf  ("[finish  set_step_size  ()]\n"); 

ret-rn; 

/•»*»«»«»♦»*»****«***«**»»«*********«**»*««*«»**««*««*♦******♦»******«*««««*««*«/ 

void  tty_ir,ode  ( tty_mode_path,  mode) 

int   tty_mode_path; 

int    mode;  /*  note  type  specifications  differ  from  headtest.c  */ 

static  struct  sgbuf  old, new; 
static  int  init  =  1; 
int  status; 

if  (!  SONARINSTALLED) 
1 

if  (TRACE  kU   DISPLAYSCREEN) 

printf  ("(start/stop  tty_mode,  SONARINSTALLED  false  ()]\n"); 

return; 
) 

if  (init) 
{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start   tty_roode  ())\n"); 

init    =   0; 

status   =  _gs_opt (tty_mode_path,    fcold) ; 

status   =   _gs_opt (tty_mode_path,    tnew) ; 

new. sg_c lass      =    0; 
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new 
new 
new 
new 
new 
new 
new 
new- 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 
new 


,  sg_case 

,  sg_back 

, sg_dele 

,  sg_ecnc 

. sg_al f 

, sg_null 

, sg_paus 

sg_page 

, sg_bspc 

sg_dlnc 

sg_eorc 

sg_eof c 

sg_rlnc 

sg_duln 

sg_psch 

, sg_kbic 

, sg_kbac 

, sq_bsec 

sg_bell 

sg_pari 

£g_taDC 

sg_tab3 

sg_tbl 

sg_ccl 

sg_err 


=  0 

sp  =  0 

te  =  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 


ch  =  0 
=  0 


h 

h 

h 

ch 

cy 

r 


=  0 

=  C 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 

=  0 


if    (mode)  _ss_opu  ( tty_inode_path,  &new)  ; 
else        _ss_opr  (cty_inode_path,  &old!  ; 

jf  [TPJiCE   i.4r  DISPLAYSCREEN)  printf  ("[finish  tcy_inode  ()]\n' 

return; 


■•**«««»**»«***i 


r**««**ir»*: 


■«***«***«»**w*«««i 


void  print_valid_keywords  () 


\r. ' 


l-J !  .  ^  i. 

; .  " 

tr'  -  -•  "  -  - 

prir.rf 

■    t* 

prinrf 

{   " 

printf 

(  " 

printf 

;  " 

,,,-;_,.  f 

1     H 

IJa.  ^1.^  . 

printf 

'  " 

printf 

.  N 

printf 

;  " 

printf 

(  " 

printf 

I  " 

printf 

("See 

printf 

i" \n") 

[help]  [trace ! notrace]  [loopforever 
[wait  #1  [time  #]  [timestep  (0.0..5 
[ keyboard  I keyboard-off ]  [quit]  [kil 
[rpm]  [course]  [depth]  [thrustersit 
[ loopf ilebackup]  [enter control const 
[rotate]  [position]  [orientation] \n 
[sonar trace l sonar traceoff ]  [sonar in 
[ trace  1  trace- off]  [parallel port trac 
[remotehost  hostname]  [realtime  I nopa 
[ loop- forever  I  loop- once] [entercontr 
[silence]  [e-mail  I  no-email ]  [waypoin 
-/execution/mission. script .HELP  for  command  synt 


I looponce] \n" ) ; 
.0 ) ]  [mission] \n" ) 
1] \n-)  ; 

hrusters-of f ] \n" ) ; 
ants] \n' ) ; 


stalled] \n 
e]  \n"); 
use]  (paus 
olconstant 
t] \n\n') ; 
ax  details 


•)  ; 

e]\n'); 
s)\n\n') 

.\n'); 


#if  defined(sgi) 

f>rintf  ("popping  up  'mission  .script  .HELP'  as  a  zip  file...\n' 
system  ("zip  -v  -/execution/mission. script .HELP" ) ; 

#endif 

return; 


)  /•  end  print_valid_keywords  */ 


/ 


»»»*»»»«*•***««*»«**«** 


**«**♦*•*****••****«***»******♦***•*»»»•»»»••»«•«*»*••*/ 


void  open_virtual_world_socket  () 


/•  see  os9sender.c  for  original  code  */ 
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{ 

if  (TRACE  (,(,    DISPLAYSCREEN) 

printf  ("(start  operj_virtual_world_socket  {)]\n"); 

/*  Initialize  communications  blocks  */ 

/•  Initialize  both  client  &  server  ••»»•••*••*»♦••••••*•»»*••••«•**•*•»*••*•♦»»/ 

/*  Signal  handlers  for  termination  to  override  net_open  0  and  net_close  ()*/ 
/*     signal  handlers.   Otherwise  you  are  unable  to  '"C  kill  this  program.   */ 


*.f   defined ;3gi) 

signal  (SIGHUP,  shutdown_virtual_world_socket ) 

signal  (SIGINT,  shutdown_virtual_world_socket ) 

signal  (SIGKILL,  shutdown_virtual_world_socket ) 

signal  (EIGFIPE,  shutdown_virtual_world_socket ) 

sigi'ial  (SIGTEFIK,  shutdown_virtual_world_socket ) 
«eijdi  f 


/•  hangup  */ 

/*  interrupt  character  */ 

/•  kill  signal  from  Unix  */ 

/*  broken  pipe  from  other  host*/ 

/*  software  termination  •/ 


/»  ''pic  ^ali'^e  sender  *«*«•***«*«*«»«***•••«*•*»*******«♦**«**«»**************«*/ 

.•  ■*  .^tart  by  fmdina  default /desired  remote  host  to  connect  to  */ 

{ 

server_enti ty  =  gethostbyname  (virtual_world_remote_host_name) ; 
if  (server_enti ty  ==  NULL) 

if  (E'ISPLAYSCREEN; 
{ 

pr int f I " lopen_vi rtual_world_socket :   virtual  world  remote  host\n"); 

printf ("  (\"%s\")  not  found]\n', 

virtual_world_remote_host_name) ; 

f flush  ( stdout ) ; 
} 

/*  error  message  needed  on  (open)  output  file   <<«<<«<<««<<<<<<  */ 
vi rtual_world_socket_opened  =  FALSE; 
return; 

else  if  (TRA7E  tS.  DISPLAYSCREEN  i 

printf (" iopen_virtuai_world_socket :   virtual  world  remote  host  *); 
printf ("( \ "%s\ " )  located] \n",   virtual_world_remote_host_name) ; 


Client  opens  server  port 


/*  Fill  m  structure  'server_address'  with  the  address  of  the  */ 

/*         remote  host  (i.e.  SERVER)  that  we  want  to  connect  with:       •/ 

*i  f  def  med  (sgi  ) 

bzero  ((char  •)  i.server_address ,  sizeof  (server_address)  )  ; 
«endif 

server_address . sin_f amily   =  AF_INET;      /•  Internet  protocol  family  •/ 

/*  copy  server  IP  address  into  sockaddr_in  struct  server_address        •/ 
«if  defined (sgi ) 

bcopy  (server_entity->h_addr,  & (server_address . sin_addr .s_addr) , 
server_entity->h_length) ; 
«else 

strncpy  (i.  (server_address  .  sin_addr  .s_addr)  ,  server_entity->h_addr, 
server_entity->h_length) ; 
«endif 

/•  make  sure  port  is  in  network  byte  order  */ 

server_address.sin_port  =  htons  (AUVSIM1_TCP_P0RT_1 ) ; 

/•  Open  TCP  (Internet  stream)  socket  */ 
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It     I  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 
I 

if  (DISPLAYSCREEN) 
( 

princf  ( " [open_vircual_world_socket :   client  can't  open  server"); 
printf  ( "  virtual  world  stream  socket]"); 
) 

/*  error  message  needed  on  (open)  output  file  «<<<<««<«<««««  •/ 
virtual_world_socket_opened  =  FALSE; 
return ; 
) 

else  if  (TRACE  &&  DISPLAYSCREEN) 
{ 

printf  ( " lopen_virtual_world_socket :   client  opened"); 
printf  ("  virtual  world  server  socket  successfully ] \n" ) ; 
) 

/'  Connect  to  the  server.   Process  will  block/sleep  until  connection  is 

IS  established.   Timeout  will  return  an  error.  */ 

if  (connect  (  socket_descriptor , 

(struct  sockaddr  *)  &server_address, 

sizecf  (server_address) )  <  0) 
i 

if  ^DISPLAYSCREEN) 
i 

printf  ( " (open_virtual_world_socket :   client  can't  connect  to"); 

printf  ("  virtual  world  server  socket] \n"); 
1 

/'  error  message  needed  on  (open)  output  file  «<<«<<<<<<«««««  •/ 

virtuai_worid_socket_opened  =  FALSE; 
ret-r.'', ; 
I 

eaSe  if  (TRACE  &i  DISPLAYSCREEN) 

printf  (• [execution  client  connected  to  virtual  world"); 
printf  ("  server  socket  successfully ]  \n"  )  ; 
i 

virtuaj_world_socket_opened  =  TRUE; 

■•  /'  end  initialization  */ 

SOCK. et_st ream  =  socket_descriptor;    /*  client  */ 

if  (TRACE  &&  DISPLAYSCREEN)   /*  print  final  info  */ 
( 
printf  "•  lopen_virtual_world_socket  CLIENT:   socket_descriptor  =  %d]\n', 

socket_descriptor) ; 
printf ("[  socket_accepted   =  %d]\n", 

socket_accepted) ; 
printf ("[  socket_stream     =  %d)\n", 

socket_stream) ; 
J 

if  (TRACE  (.(.    DISPLAYSCREEN) 

printf  ("[finish  open_virtual_world_socket  ()]\n"); 

return; 

)/•  end  open_virtual_world_socket  ()  */ 

void  shutdown_virtual_world_socket  ()    /•  see  os9sender.c  for  original  code  •/ 
{ 

int   kill_return_value; 

shutdown_signal_received  =  TRUE; 
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if  (virtuaI_world_socket_opened  ==  FALSE) 

it     (TRACE  &&  DISPLAYSCREEN) 
{ 

princf  ( " Ivirtual_world_socket_opened  FALSE,"); 
pi'incf  ('  shutdown_virtual_world_soc)cet  ignored]  \n* )  ; 
} 

return; 
) 
if  (TRACE  is,  DISPLAYSCREEN) 

princf  (  "  (  shucdown_vircual_world_soc)cec  start  ...]\n"); 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down,      •/ 
/'    since  SIGPIPE  signal  trigger  may  shutdown  server  on  other  side       */ 

if  (cJcse  {soc)<et_st ream)  ==  -1) 

if  (TRACE  i,l>    DISPLAYSCREEN! 
printf  ( "shutdown_virtuai_world_socket  close  (socket_st ream)  failedXn"); 

/'  shutdown  ()  reference:   'Using  OS-9  Internet"  manual  p.  2-55       •/ 

if  (shutdowr,  ( socket_strean-i,  2)  ==  -1) 

if  (TRACE  SrSr  DISPLAYSCREEN) 

printf  (  •  [sh'Jtdown_virtual_world_socket  shutdovTi" )  ; 
print:  ("  (socket_stream,  2)  failed] \n"); 

kill_return_value  =  kill  (socket_stream,  SIGKILL) ; 

if  (TRACE  i&  DISPLAYSCREEN) 

printf  ( • [shutdown_virtual_world_socket  kill  ( socket_stream, " ) ; 
printf  ("  SIGKILL)  returned  %d]\n",  kill_return_value) ; 


if  'TRACE  6r&  DISPLAYSCREEN) 

printf  {" [ shutdown_virtuai_world_socket  return] \n") ; 

return ; 

1  ,' *  end  s.'".utdown_vi  rtual_world_socket  ()  •/ 
,«•»»*»»»»»»»»»♦«**»«*♦*««*«**«»*«*■»«*****«»*»***«««****♦«♦**««*««*««**«»««*****/ 

void  ?er;d_d3ta_on_virtua2_world_socket  ()/*  see  os9sender.c  for  original  code  •/ 
1 

byte£_left        =  socket_length; 

by te3_written     =  0; 

ptr_index        =  buffer;   /•  this  global  string  is  the  data  to  be  sent  */ 

if  (virtual_world_socket_opened  ==  FALSE) 
i 

return; 
) 
if  (TRACE  &S-  DISPLAYSCREEN) 

printf  ( • lsend_data_on_virtual_world_socket  start  ...]\n'); 

while  ((bytes_left  >  0)  &£<  (bytes_written  >=  0))   /*  write  loop  »**••••♦***/ 

bytes_sent  =  write  (socket_stream,  ptr_index,  bytes_lef t ) ; 

if      (bytes_sent  <  0)  bytes_written  =  bytes_sent; 
else  if  (bytes_sent  >  0) 
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byt:es_lefc  -=  bytes_sent; 

bytes_wri  tten  +=  byces_sent:; 

pcr_index  *=  bytes_sent; 
1 

if  (LOi.ATIDNLAE  l.i.    TRACE  is.  DI SPLAYSCREEN) 
{ 

printf  (  "  (  record_data  send_teleinetry_to_server  loop"); 

prmcf  ("   bytes  sent  =  %d]\n",  bytes_sent )  ; 
1 

if    iby tes_written  <  0) 

if  (LOCATIONLAB  Ui,    DISPLAYSCREEN) 
{ 

printf  (  "  [  record_data  send_telenietry_to_server  ()  send  failed,  "); 

printf  ('%d  bytes_written] \n" ,  bytes_wri tten) ; 
) 
/•  error  message  needed  on  (open)  output  file   «««««««««««  */ 

else  if  (LOCATIONLAB  i&  TRACE  &&  DISPLAYSCREEN) 
( 

printf    ( "  1  record_data    send_teleinetry_to_server   total    bytes   sent"); 

printf  ("  =  %dl\n",  bytes_written ) ; 
1 


neck    teriTiination 


if    'strncinp    (buffer,    "shutdown",    8)    ==    0) 

If  (TRACE  S.&  DISPLAYSCREEN,  printf 

' " i  send_data_on_virtuai_world_socket :  shutdown_signal_received] \n" ) ; 

shutdcwn_vi  r  tua2_wcr  Id_soc)':er  ()  ; 

i:  (TRACE  ii  DISPLAYSCREEN; 

printf  .  "  lsend_data_on_virtuai_world_soc)<et  return]  \n"  )  ; 

ret'L^rn  ; 

•    /*    end   ser;G_data_on_vi  rtual_wor  id_socket    i;    ■*  / 

void  get_st  rean'i_f  roni_virtual_world_soc)^et    ()      /*    see   os9sender.c   for   original    •/ 
{ 

if    ( VI  rtuai_world_soc)'iet_opened  =-   FALSE) 
i 

return; 
i 
if  (TRACE  &£,  DISPLAYSCREEN) 

printf    ( "  [get_stream_f  rorr,_virtual_world_socket    start    ...]\n"); 

/•  listen  to  remote  host,  relay  to  local  network /program  «/ 

bytes_left        =  socket_length; 

bytes_received    =  0; 

ptr_index        =  buf f er_received; 

while  ((bytes_left  >  0)  &&  (bytes_received  >=  0))   /♦  read  loop  ••»•«*«••*•«/ 
{ 

bytes_read  =    read    (socket_streaiii,    ptr_index,    bytes_left); 

if      (bytes_read  <  0)  bytes_received  =  bytes_read; 

else   if    (bytes_read   >      0) 

{ 

bytes_left  -=  bytes_read; 

by tes_received   *=   bytes_read; 
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pcr_index      +=  bytes_read; 
) 

if  (TRATE  ii  DISPLAYSCREEN/ 
1 

print  f  { °  (get_stream_f  roni_virtual_world_socket  receiver  block*); 
printf  ("  loop  bytes_read  =  %d]\n",  bytes_read) ; 
) 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  (  (bytes_read  ==  0)  iSr  (bytes_received  ==  0))  break; 
i 

if   (bytes_received  <  0)       /*  failure     */ 
i 

if  (TRACE  S,i  DISPLAYSCREEN) 
{ 

printf  ( "  [get_stream_f  roiT._virtual_world_sccket  receiver  block  read'); 
printf  ("  failed,  bytes_received  =  %d\n",  bytes_received) ; 
) 
) 
else  if  ;bytes_recfeived  ==  0)   /•  no  transfer  •/ 

if  (TR-ACE  6,^  DISPLAYSCREEN) 

print  f  ■"  i  aet_streaiTi_f  roiT:_virtual_world_socket  received  0  bytes  !!)  \n" )  ; 

) 

el?e    if     ; j:\-tes_received   >   Oj       /*    success  •/ 

if     (TRACE    ii    DISPLAYSCREEN) 

print  f  I "  lget_st  reaiT._f  rom_virtual_world_socket    received   %d  bytes]  \n", 

by tes_received I ; 
I 

) 
/'    Check    teiTP.i nation    •*•••«»»•«••«»*••••••••*•••**«««•**•••••••••«••••«•••••/ 

if    istrncr.p    ibuf fer_received,    "shutdown",    8)    ==   0) 
( 

if     (TRACE    &&    DISPLAYSCREEN,    printf 

I " lget_data_on_virtual_world_socket :    shutdown_signal_received) \n" ) ; 

shutdcwn_virtuai_world_socket    () ; 

if     'TRACE    6<&    DISPLAYSCREEN) 

printf    (  "  [get_streaip_f  roiri_virtual_world_socket    return)  \n"); 

r<r  t'j  rn ; 

/•    end   get_streaiT;_f roin_virtual_world_socket    ()    */ 


double  degrees    (rads)  /*    radians   input    •/ 

double      rads; 
{ 

return   rads    '    180.0    /    PI; 

) 

/***••* ,..............,,....,...,...,,,..,...,.,......,.,/ 

double  radians  (degs)     /*  degrees  input*/ 

double  degs; 
{ 

return  degs    *    PI    /    180.0; 
) 


double  normalize    (degs)  /*   degrees   input"/ 
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double  degs; 
{ 

dcjtie  result  =  degs; 

whale  (resulc  <  0.0)  result  +=  360.0; 

w;-,.le  (result  >=  360. 0)  result  -=  360.0; 

return  result; 

) 

/*«*♦*»■»***♦«****««♦**««*»**«*«»*****«»*«»****♦«»***»«****♦************♦******»/ 

double  normalize2    (degs)  /*   degrees    input*/ 

douDle      deqs; 
( 

double  result  =  degs; 

while  (result  <=  -180.0)  result  +=  360.0; 
wn.le  (result  --    180.0)  result  -=  360.0; 

return  result; 

) 

/»«*»■»»««»**»»«*«««»««**««»***»««**»«*»*««*»**«*****«♦«***«***«*«****«««*♦♦**«*/ 

double  radian_normalize  (rads)     /*  radians  input*/ 
double  rads; 

{ 

double  result  -    rads; 

while  (result  <       0.0)  result  *-    2.0  *  PI; 
wh.le  (result  >-  2.0  *  PI)  result  -=  2.0  *  PI; 

return  result; 
\ 

double  radian_normal i ze2  (rads)     /*  radians  input*/ 

double   rads; 
( 

douhle  result  -    rads; 

while  (result  <=  -  PI)  result  +=  2.0  *  PI; 
while  (result  >    PI)  result  -=  2.0  *  PI; 

} 

void  clamp  fclampee,  absolute_min,  absolute_max,  name) 

double    *  clampee; 

double  absolute_min; 

double  absolute_max; 

char   *  name; 
( 

double  new_value,  locaI_min,  local_max; 

if  ( (absolute_max  ==  0.0)  &&  (absolute_min  ==  0.0))  return;     /*  no  clamp  */ 

if  (absolute_max  >=  absolute_min)  /*  ensure  min  &  max  used  in  proper  order  */ 
( 

local_min  =  absolute_min; 

local_max  =  absolute_max; 
) 

el  se 
( 

local_min  =  absolute_max; 

local_max  =  absolute_min; 
) 
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if  ''*  clampeei  >  local_max) 
( 

new_volue   =    local_rr.ax  ; 

li     (TRACE  4.6r  DISPLAYSCREEN) 

print!  ("[clamping  %s  froip  %5.3f  to  %5.3f]\n", 
name,  *  clampee,  new_value) ; 

•  clampee  =  new_value; 
} 

if  ((•  clampee)  <   local_min) 
i 

new_vaiU€  =  local_min; 

If  (TRACE  &i  DISPLAYSCREEN I 

printf  ("[clamping  %s  from  %5.3f  to  %5.3f]\n", 
name,  *  clampee,  new_value) ; 

'  clampee  =  new_value; 

I 

,' '  t.'.anks  to  M:chael  Olberg    Oct  20,  94    olberg@bele.oso.chalmers.se      '/ 

dr.'^Ll-^  atanZ  :y ,  x  , 

3c-..;Dle  y  ;  douLie  x  ; 

if  'TRACE  UU    DISPLAYSCREEN) 

printf  ("[atan2  (%5.3f,  %5.3f)]\n",  y,  x); 

if  y.   ==  G.g:  { 

it    ;y   •■   0.0";  return  , -PI/2  .0)  ; 

else  return  ;    PI/2 .0) ; 

1    else    i 

if    (X  <   0.0*    { 

if  !y  -^  0.0)  return  (atan  ;y/x! -PI )  ; 
else         return (atan (y/xj +PI ) ; 
■'  else  return  (atan  (y/x;  ;  ; 

} 

"•  as   tc  the  tann  you  will  simply  have  to  use  •/ 

double  sinh (x; 

douoie  X; 
( 

return  (fexp!x)  -  exp ( -x) ) /2 . 0; 
} 

double  cosh  (x; 

double  X; 

return  (exp(x)  +  exp (-x) ) /2 .0 ; 
} 

double  tanh(x) 

double  X; 
{ 

return  sinh (x) /cosh (x) ; 
) 

#endif 
/»»»♦♦♦*•♦♦««*♦♦♦♦**«♦♦**♦«♦♦♦**♦♦♦*♦***«**♦**«****♦♦*♦**♦****♦*******«*♦♦«♦***/ 


73 


douiile   Sign    (x) 
double  X, 


if 

(X 

> 

C 

0) 

else 

if 

(X 

< 

0 

0) 

else 

return  1  . 0 
return  -1.0 
return      0.0 


»  *  ♦ 

♦  * 

*  » 

*  *  * 

•  * 

/  * 

«  »  * 

♦  ♦ 

• . 

*«***«* 

*  *  ■» 

*  * 

w**-****-* 

/  * 

er 

J 

cf 

execut 

iOI". 

.c 

/• 

«  «  « 

t  * 

•K    * 

»**«**« 

■»  ■*  « 

*  * 

«**♦*«*« 

/* 

»  ■»  « 

w  * 

*  * 

*«**•*«* 

«  ■«  « 

*  * 

******** 

■**♦*********«**♦*«****«**♦***««************/ 
■**************************«********«***♦«**/ 

*/ 

■*******«**««***«***««*****************«»«**/ 
■*«**************«****♦******««*************/ 
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D. 


parse _f unctions. c  Tactical  Script  Command  Parse  Functions 


Program: 

Authors : 

Revised: 

System: 
Comp: ler : 

Ccmp: lat ion: 

168020  1 
I68C30] 


r  ■**♦***  1 


■*»♦**■ 


**«*««**********i 


•***»******«*** 


«  *  «  *  / 


[Irix  ] 


F'- 1-\ 


parse_f unctions . c 

Don  Brutzman 

28  October  94 

AW  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc   Kernighan  &  Richie  (K&R)  C 

ftp>     put  parse_functions . c 
auvsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make      execution 

fletch>  make  execution 

Reduce  size  of  execution. c  to  allow  OS-9  C  compiler  to  work 


/'  parse_funct: ons . c  »/ 

♦include  "globals.h" 
#::".ciud»^  "def  ines  .  h  ' 

void  parse_com.Tiand_line_f  lags 

vcid  parse_mission_script_commands 

void  parse_mission_string_commands 

extern  void  print_valid_keywords 

extern  void  send_data_on_virtuai_world_socket 

extern  void  get_control_constants 

extern  void  clamp 


■««««* 


■**«****««T*«****«***«*»»*«««**«*«**«**»**«**: 


vr:d  parse_com]T.and_line_f lags    (argc,    argv) 

int    argc;    char    ''argv;  /•    comiriand   line   arguments    */ 

i 

int    index; 

if  (DISPLAYSCREEN) 
t 

printf  ( " [parse_command_line_f lags  start:  *  arguments  =  %d]\n[',  argc) ; 

for  (I  =  0;  i  <  argc;  i  +  -r)  printf  c  %s",  argv[i]); 

printf  (•  l\n"); 
} 

if  (DISPLAYSCREEN)  printf  ('[parse  arguments:   •); 


( 


for  (i  =  1;  i  <  argc;  ii-+) 

printf  (•%s  ',  argv[i]); 

for  (index  =  0;  index  <=  (int)strlen  (argv[i]);  index+4)/*  uppercase  •/ 
argv[i]  [index]  =  toupper  (argv[i]  [index]); 
} 

printf  (NXn'); 
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srrcpy  (buffer,  ' " ) ;  /*  initialize  for  SILENT  */ 

for  (i  =  1;  i  <  argc;  i++) 

if      ( (strcmp  (argv[i],   'HELP")  ==0)  II 
(strcmp  (argv[i],     •?•)  ==0)  II 
(strcmp  (argvii],     "/?■)  ==0)  II 
(strcmp  (argvii),     "-?")  ==  0)) 
1 

if  (TRACE  &£<  DISPLAYSCREEN)  printf  ( '  [print_help]  ' )  ; 
print_help  =  TRUE; 
) 

else  if  ((strcmp  (argv[i],   "KEYBOARD")      ==0)  II 
(strcmp  (arav[i],  "KEY-BOARD")       ==0)  II 
(strcmp  !argv[i],  "KEYBOARD- INPUT" )  ==0)  11 
(strcmp  (argv[i],  "KEYBOARDINPUT" )   ==  0)) 
{ 

if  (TRACE  4r&  DISPLAYSCREEN)  printf  ("[KEYBOARDINPUT  =  TRUE]  " )  ; 
KEYBOARDINPUT  =  TRUE; 
1 

else  if  (strcmp  (argvii),   "TRACE";  -=    0) 
{ 

if  (TRACE  &£.  DISPLAYSCREEN i  printf  ("  [TRACE  =  TRUE]  "); 
TRACE  =  TRUE; 
i 

else  if  ((strcmp  (argv[i),   "TRACEOFF")  ==0)  II 
(strcmp  (argv[i;,  "TRACE-OFF")  ==0)  II 
(strcmp  (argvii j,    "NOTRACE")  ==0)  II 
(strcmp  (argvii),   "NO-TRACE")  ==  0)) 
i 

if  (TRACE  ici  DISPLAYSCREEN)  printf  ("[TRACE  =  FALSE]  "); 
TRACE  =  FALSE; 
) 

else  :f  : (strcmp  (argvii),   "LOOPFOREVER" )  ==0)  II 
(Strcmp  (argvii),  "  LOOP- FOREVER '  )  ==  On 

if  (TRACE  5,^  DISPLAYSCREEN;  printf  ("[LOOPFOREVER]  "); 

LOOPFOREVER  =  TRUE; 
.1 

else  if  ((strcmp  (argvii),   "LOOPONCE")  ==0)  II 
(strcmp  (argvii],  "LOOP-ONCE")  ==  0)) 

if  (TRACE  &£■  DISPLAYSCREEN)  printf  ("[LOOPONCE]  "); 

LOOPFOREVER  =  FALSE; 

else  if  ((strcmp  (arqv[i],    "LOOPFILEBACKUP" )  =^0)  II 

(strcmp  (argvii],  "LOOP-FILE-BACKUP")  ==0)) 
{ 

if  (TRACE  ii  DISPLAYSCREEN)  printf  ("[LOOPFILEBACKUP]  "); 

LOOPFILEBACKUP  =  TRUE; 
} 
else  if  ((strcmp  (argv[i],    " ENTERCONTROLCONSTANTS " )  ==0)  li 

(strcmp  (argv[i],  "ENTER-CONTRGL-CONSTANTS" )  ==  0)) 
{ 

if  (TRACE  &£.  DISPLAYSCREEN)  printf  (■[ENTERCONTROLCONSTANTS]  ') 

ENTERCONTROLCONSTANTS  =  TRUE; 

) 

else   if  ((strcmp  (argv[i],   "TACTICAL")     ==0)  || 

(strcmp  (argvii],   "TACTICAL-GN" )  ==  0)) 
{ 

printf  ("[%s]\n",  argv[i)); 
TACTICAL  =  TRUE; 
) 

else   if  ((strcmp  (argvii],   "NO-TACTICAL" )   ==0)  II 
(strcmp  (argvii),   "TACTICAL-OFF" )  ==0)) 
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{ 

printf  {"l^slXn",  argv[i]); 
TACTICAL  =  FALSE; 
} 

e]se  if  ' (strcmp  {argv[i],   • SONARTRACE ■ )  ==0)  II 

(strcmp  (argvli],  • SONAR- TRACE " )  ==  0)) 
{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('[SONARTRACE]  '); 

SONARTRACE  =  TRUE; 
) 
else  if  ((strcmp  (argv[i],    "SONARTRACEOFF" )  ==0)  II 

(strcmp  (argv[i],  'SONAR-TRACE-OFF' )  ==  0)) 
t 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ( ' [ SONARTRACEOFF]  ' ) ; 

SONARTRACE  =  FALSE; 
) 
else  if  ( istrcmp  (argv(i],   "SONARINSTALLED" )  ==0)  II 

(strcmp  (argvli],  "SONAR- INSTALLED" )  ==  0)) 
( 

if  (TRACE  UU    DISPLAYSCREEN)  printf  (" [SONAR INSTALLED]  "); 

SONAR INSTALLED  =  TRUE; 

else  if  ((strcmp  (argv[i],    "PARALLELPORTTRACE" )  ==0)  II 
(strcmp  (argv[i],  " PARALLEL- PORT-TRACE " )  ==  0)) 

If  .'TRACE  Ui.    DISPLAYSCREEN:  printf  ("[PARALLELPORTTRACE]  "); 
PARALLELPORTTRACE  =  TRUE; 

-:.-e  .f  VL-ticnp  (argvli],   "SILENT")  ==0)  II 
(Strcmp  largvli],  "SILENCE")  ==  0)) 

if  (TRACE  &i  DISPLAYSCREEN)  printf  ("[SILENT]  "); 

/'  send  tc  virtual  world  after  socket  is  open  •/ 

strcpy  (Duffer,  "SILENT");  / '  copy  current  comitiand  to  buffer  */ 

else  if  ((Strcmp  (argv[i],   "TIMESTEP'j  ==0)  II 

(strcmp  (argv[ij,  "TIME-STEP")  ==  0)) 
i 

■^  *  "*■  / 

if    (i    >=   argc)    print_help   =   TRUE; 

else 

( 

sscanf  (argv[i],  "%F",  &TIMESTEP) ; 

if  iTRACE  &&  DISPLAYSCREEN)  printf (" [TIMESTEP  %f]",  TIMESTEP); 

if  (TIMESTEP  >  CO)  dt  =  TIMESTEP; 

else  if  (TRACE  S.S,  DISPLAYSCREEN) 

printf  ("   illegal  TIMESTEP  value,  ignored."); 
if  (TRUE  i&  DISPLAYSCREEN)  printf ( "   [dt  =   %f]",  dt ) ; 
) 
I 

else  if  ((strcmp  (argv[i],   "REMOTEHOST" )  ==0)  II 
(strcmp  (argv[i],  " REMOTE- HOST " )  ==0)  II 
(strcmp  (argv[i],   "REMOTE")      ==0)  II 
(strcmp  (argv[i],   "HOST")       ==  0)) 
{ 

i*  + ; 

if  (i  >=  argc)  print_help  =  TRUE; 

else 

( 

sscanf  (argvli],  ■%s",   virtual_world_remote_host_name) ; 
if  (TRACE  &<&  DISPLAYSCREEN) 

printf (• [REMOTE-HOST  %s]",  virtual_world_remote_host_name) 
) 
) 
else  if  ((strcmp  (argv[i],   "REALTIME")  ==0)  II 

(strcmp  (argv[i],  "REAL-TIME")  ==  0)) 
{ 
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if  (TRACE  &S.  DISPLAYSCREEN)  printf  C  [REALTIME] 
REALTIME  =  TRUE; 
) 
else  if  ( (srrcmp  (argvli] 

(strcmp  (argvli] 

(strcmp  (argvli) 

(strcmp  (argvli) 

(strcmp  (argvli) 

(strcmp  (argvli) 

(strcmp  (argvli) 


)  ; 


,       'NOREALTIME') 

==0)        II 

"NO-REALTIME") 

==0)        II 

•NO-REAL-TIME") 

==0)        II 

"NOWAIT") 

==0)        II 

,     "NO-WAIT") 

==0)        II 

,       "NO PAUSE") 

==0)        II 

,     "NO- PAUSE") 

=  =    0)) 

( 


if  (TRACE  iS.  DISPLAYSCREEN)  princf  ("[NOWAIT) 
REALTIME  =  FALSE; 


) 

else  if  ((strcmp  (argvli),   "NOEMAIL")   ==0)  M 

(strcmp  (argvli),   "NO-EMAIL")  ==  0)) 
( 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[NO  EMAIL) 

EMAIL  ^  FALSE; 


)  ; 


ei?e  pr:nt_help  =  TRUE;   / *  invalid  command  line  entry  parameter  found  */ 
!       /'  end  fcr  Icop  through  command  line  paramieters  */ 

i:  ;print_help)  /*  print  help  string  •»'«««**««••«*"■•♦«••*•••**••«••♦*••*««/ 

i 

print f ("  \nUsage :   execution  \n"); 
print_val id_keywcrds  (); 
exit  i  -  i  ,  ; 

if  (TRACE  £<&  DISPLAYSCREEN;  printf  { "  \n  [parse_command_line_f  lags  complete)  \n' ) 
return; 

i  /'  end  parse_coiTini3nd_line_f lags  ()  */ 

V'.i  , 


.i:-?e_:T,i  ssion_script_commands  ()   /■*  get  data  from  file  at  program  start  */ 

f   mission . script .HELP  =>  descriptions  •/ 


mt    index,  read_another_l  ine,  paramieters_read; 
jr.,.-   parameterl ,  parameter2,  parameters,  parameter^,  parameters; 

cnar   bcckupcommiand  [50],  new_filename  [30]; 

if  (TRACE  S.i  DISPLAYSCREEN) 

printf  ("[start  parse_mission_script_commands  ()]\n"); 


if 
i 


(auvscriptf lie  ==  NULL) 


feof  (auvscriptf ile) 


auvscriptf ilequ it ) 


if  (DISPLAYSCREEN) 
{ 

printf  {"[opening  a  copy  of  the  auvscriptf ile  %s]\n", 

AUVSCRIPTF I LENAME) ; 
f flush  (stdout ) ; 
) 
#if  defined(sgi) 

sprintf  (backupcommand,  "rm      mission. script .backup* ) ; 

printf  ("%s\n",  backupcommand); 
system   (backupcommand) ; 

sprintf  (backupcommand,  'cp   %s  mission. script .backup" , 
AUVSCRIPTFILENAME) ; 
printf  (•%s\n",  backupcommand); 
system   (backupcommand) ; 


*else 


/*  OS-9  */ 
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«.rnJ^  f 


sprincf  (backupcoirmand,  "del     mission  .  script  .  backup' )  ; 

pi'intf  (■%s\n",  backupcommand)  ; 

sysceni  (backupcommand)  ; 

spxintf  (backupcommand,  "copy  %s  rrassion  .  script  .backup"  , 
AUVSCRIPTFILENAJ-IE!  , 

printf  i*%s\n",  backupcommand); 

system  (backupcomrriand)  ; 

auvscript file  =  fopen  ( "mission . script . backup" , "r* ) ;   /*  input  file  */ 

if  (auvscript file  ==  NULL) 

{ 

printf  CAUV  execution:   script  file  %s\n",  AUVSCRIPTFILENAME) ; 

printf 
("  (or  backup  copy  mission. script .backup)  not  found. \n") 

printf  (■  Ensure  you  are  in  the  right  directory : \n' ) 

printf  ("  auvsiml>  chd  /hO/execution       or\n'); 

printf  (•  unix>    cd  -brutzman/executionXn" ) ; 

printf  { "Exit .  \n" ) ; 

exit  (-1 i ; 
) 
auvscriptfileguit  =  FALSE; 

sprmtf  (buffer,  •%s.  backup",  AUVORDERSFILENAME)  ; 
auvordersf ile  =  fopen  (buffer, "w");   /•  output  file  •/ 
if  'TRACE  S.&  DISFLAYSCREEN) 

printf  ( "auvordersf lie  (%s)  =  %x\n",  AUVORDERSFILENAME, 
auvordersf 1 ie) ; 
if  ■■  auvordersf- ie  ==  NULL! 
( 

printf  (  "AU'w'  execution:   %p  file  not  opened.  \n",  buffer); 
prir.t:  ("  Error.  \n"); 

printf  {  "Exi  t  .  \n"  ;•  ; 
exit  '-1 ) ; 
} 
it     (TRACE  i&  E>ISFLAYSCREENi 

piintf  (" [auvordersf lie  =  %x,  opened  succesfully ] \n" , 
auvordersf 1 ie; ; 

fprintf  ( auvordersf ile. 


fpiintf  (auvordersf ile, 
"#  NFS  A'.TV  file  %s:  commanded  propulsion  orders  versus  timeXn", 
AUVORDERSFILENAME) ; 

fprintf  (auvordersf ile, 
" «  V  n  ■ ;  ; 

fprintf  (auvordersf ile, 
"#         timestep:   %4.2f  seconds \n",  dt); 

fprintf  (auvordersf ile, 
■ « \  n " ; ; 

fprintf  (auvordersf ile, 
"«  time  heading  North  East  Depth    rpm   rpm    stern  stern   vertical 
lateral  \n' ) ; 

fprintf  (auvordersf ile, 
•«  X    y     2      port  stbd   plane  rudder  thrusters 

chrusters\n" ) ; 

fprintf  (auvordersf ile, 
■#  bow/stern 

bow/sternXn" ) ; 

fprintf  (auvordersf ile, 
•\n" )  ; 

) 

else  if  (TRACE  *.&  DISFLAYSCREEN) 

printf  ( • (auvscriptf ile  checks  out  as  ready ...) \n" ) ; 

read_another_line  =  TRUE; 
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while  (read_another_line  ==  TRUE)    /•  «•*»»•*••  Parse  loop 
{ 

paramecerl  =  0.0; 

paraineter2    =    0.0 

paraiTieterJ    =    0.0 

:f  (KEYBOA.RDINPUT  ==  TRUE) 

strcpy  (buffer,  "Enter  mission  script  command"); 
send_data_on_virtual_world_socket  ();  /•  buffer  msg  sent  •/ 
pr:ntf  ("\n%s  *»*  HERE  •**:   ",  buffer); 
get?  (command_buf f er ) ; 
) 

else 
I 

strcpy    (command_buf fer ,    ""); 

fgets      (cominand_buf  fer ,    120,    auvscriptf  ile)  ; 

if  (feof  (auvscraptf lie) ) 
1 

if  (DISPLAYSCREEN) 
printf  ("[EOF  condition:   (%s  copy)  mission . script . backup,  file  closed] \n' 

AUVSCRIPTFILENAME) ; 
f close  (auvscriptf lie) ; 
auvscriptf ilequit  =  TRUE; 
read_anotrier_line  =  FALSE; 
end_test  =  TRUE; 

strcpy  (con"iiriand_buf  f  er,  ""); 
break ; 


if  ;  (int;  (strlen  (command_buf fer)  <=  120)  &&  TRACE  &s.  DISPLAYSCREEN) 

printf  ("strlen  (command_buf f er )  =  %d",  strlen  (cominand_buf fer)  )  ; 
printf  f">>>%s<<<",  command_buf f er ) ; 

p£rarrieter£_read  =  sscanf  (coiTiiTiand_buf fer,  "%s",  keyword); 

if  'TRACE  &&  DISPLAYSCREEN) 
i 

printf     i  "parameters_read   =    %d,    keyword  =    %s'', 
pararTieters_read,  keyword); 

fer  (index=0;  index<= (int ) strlen  (keyword);  index++)  /*  set  uppercase  */ 
keyword  (index)  =  toupper  (keyword  (index]); 

audible_ccirjTiand  =   TRUE; 

if  (TRACE  ii  DISPLAYSCREEN) 

printf  (",  uppercase  keyword  =  %s\n",   keyword); 

) 

if     ( (parameters_read  !=  1)  II 

(strlen  (keyword)        ==0)  II 
(strlen  (command_buf f er)   ==0)  II 

(command_buf fer  (0)       ==  '\n'))  /*  blank  line  */ 

{ 

audible_command  =  FALSE; 
printf  {'\n'); 
} 
else   if   (  keyword  (0)  ==  '#')  /*  coiranent  */ 


i 


if  (DISPLAYSCREEN)   printf  (•%s",  command_buf fer) ; 
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coiTijr.and_buf f er  [0]  =  '  '  ; 
) 

else    if  ii  (keyword  [0]  ==  '/')  &6<  (keyword  [1]  ==  '/'))  M 

((keyword  (0)  ==  '/',  ui.    (keyword  [1]  ==  '*')))  /•  commenc  */ 

if  (DISPLAYSCREEN)   princf  (•%s",  command_buf f er ) ; 
command_buf f er  [0]  =  '  '  ; 
comiriand_Duf  f  er  [1]  =  '  '  ; 
) 

else  if  ( (scrcmp  (keyword,   "HELP")  ==0)  II 
(scrcmp  (keyword,   "?")     ==0)  M 
istrcmp  (keyword,   •-?")    ==0)  M 
(strcmp  (keyword,   "/?")    ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ('[HELP]  ' ) ; 
prinL_valid_keywords  (); 
) 
else    if      ((strcmp  (keyword,   "WAIT")  ==0)  II 

(scrcmp  (keyword,   'RUN')   ==  0)) 
{ 

paraineters_read   =   sscanf    (command_buf  f  er ,    •%s%lf", 

keyword,    &   parajnecerl )  ; 
pii.ntf     ("[%s  %6.2f;    ",    keyword,    parameter  1  j  ; 

if       (  (paran-iecers_read   ==    2)    &&     (parameterl    >=    0.0)) 
i 

read_arjocher_Iir,e   =    FALSE; 

t ime_next_command  =  t  +  parameterl; 

printf  ("time  of  next  command  %6.2fj\n', 

time_next_command; ; 
fprintf  (auvordersf lie, 
-'^'..:t      "c^.-it    %5.:f  %5.1f  %5.1f  %e.lf  %6.1f  %6.1f  %6.1f   %5.1f  %5.1f  %5.1f 
%i  . -1  ,n", 

t,  psi_command,  x_command,  y_command,  z_command, 
port _r pn,_c omma nd ,  s t bd_r pm_c omma nd , 
rudder_command,  planes_command, 
bow_Iateral_thruster_comjTiand, 
stern_lateral_thruster_command, 

bow_vertical_chruster_command, 
scern_vertical_thruster_comrr.and)  ; 
} 
else  printf  ("  warning:  illegal  time  value,  ignoredXn'); 

else   if  ((strcmp  (keyword,   "TIME")       ==0)  II 
(Strcmp  (keyword,   "WAITUNTIL")   ==0)  II 
(strcmp  (keyword,   "PAUSEUNTIL")  ==  0)) 
i 

parameters_read  =  sscanf  (coTanianii_buf  f  er ,  ■%s%lf  , 

keyword,  &  parameterl); 
printf  (•l%s     %e.2f]\n',  ke^^word,  parameterl); 
if   (parameters_read  ==  2) 

read_another_line  =  FALSE; 
time_next_command  =  parameterl; 
fprintf  (auvordersf ile, 
•%f..:f   %6.1f  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f   %5.1f  %5.1f  %5.1f 
%5.1f \n", 

t,  psi_command,  x_command,  y_command,  z_coiTimand, 
port_rpm_command,  stbd_rpm_command, 
rudder_command,  planes_command, 
bow_lateral_thruster_command, 
stern_lateral_thruster_command, 

bow_vertical_thruster_command, 
stern_vertical_thruster_command) ; 
if   (parameterl  <=  t) 
( 

t  =  parameterl; 

printf  (■  warning:  time  value  has  reset  AUV  clock. \n'); 
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read_anocher_line  =  TRUE;   /*  no  PDU  */ 
) 
) 

else  printf  ("  warning:  illegal  time  value,  ignored . \n* ) ; 
) 
else  if  ( (strcmp  (keyword,  "TIMESTEP" )   ==0)  II 

(strcmp  (keyword,  'TIME-STEP")  ==  0)) 
( 

if  (sscanf  (command_buf fer,  ■%s%F",  keyword,  iparameterl)  ==  2) 
1 

if  ( (parameterl  >  0.0)  &&  (parameterl  <=  5.0)) 
{ 

dt  =  parameterl; 
If  (DISPLAYSCREEN) 

printf  ("[TIMESTEP   %6.2f]  ",  dt); 
fprintf  (auvordersf lie,  "«  timestep:   %4.2f  secondsXn",  dt) 
i 

else  print_help  -   TRUE; 
) 

else  print_help  =  TRUE; 
) 
else  if  ((strcmp  (keyword,   "PAUSE")  ==0)     M 

(strcmp  (keyword,  "-PAUSE")  ==  0)) 
{ 

if  (DISPLAYSCREEN) 
1 

printf  i " [PAUSE]  \n" ) ; 

strcpy  (buffer,  "  Press  any  key  to  continue"); 
send_data_on_virtuai_world_socket  ();  /*  buffer  msg  sent  */ 
printf  ("\n%s  •••  HERE  ***:   ",  buffer); 
answer  =  getchar  ();  /*  pause  */ 
) 

else  if  ((Strcmp  (keyword,   "REALTIME")   ==0)  11 

(Strcmp  (keyword,   "REAL-TIME")  ==0)) 
J 

if  (DISPLAYSCREEN;  printf  (" [REALTIME]  "); 
REALTIME  =  TRUE; 

else  if  ((strcmp  (keyword,  "MISSION")  ==0)  II 
(Strcmp  (keyword,  "SCRIPT")  ==0)  II 
(strcmp  (keyword,   "FILENAME")   ==0)) 


( 


parameters_read  =  sscanf  (command_buf f er ,  "%s%s", 

keyword,  new_f ilename) ; 

if   (parameters_read  ==  2) 


( 
«if  defined  (sgi 
♦  else 
#endif 


if  (DISPLAYSCREEN) 

printf  ("[%s    %s]\n",     keyword,  new_f ilename) ; 

sprintf  (backupcommand,  "cp   %s  %s",  new_f ilename, 

AUVSCRIPTFILENAME) ; 

sprintf  (backupcommand,  "copy  %s  %s",  new_f ilename, 
AUVSCRIPTFILENAME) ; 


if  (DISPLAYSCREEN) 

printf  (•%s\n",  backupcommand); 
system    (        backupcommand) ; 
) 

else 
{ 

if  (DISPLAYSCREEN) 
printf  (•  [%s)   warning:  no  filename  present,  ignored\n',  keyword); 
) 
) 
else  if  ((strcmp  (keyword,   "KEYBOARD")       ==0)  II 
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(strcmp  (keyword,   'KEYBOARD-ON" )     ==0)  II 
(strcmp  (keyword,   'KEYBOARD- INPUT" )  ==0)  II 
(strcmp  (keyword,   "KEYBOARDINPUT" )   ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("[IslXn',  keyword); 
KEYBOARDINPUT  =  TRUE ; 
) 
else  if  ((strcmp  (keyword,   'KEYBOARD-OFF" )  ==0)  II 

(strcmp  (keyword,   "NO-KEYBOARD")   ==  0)) 
( 

if  (DISPLAYSCREEN)  printf  (•[%s]\n",  keyword); 
KEYBOARDINPUT  =  FALSE; 


else  if  ((strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword. 


•NOWAIT') 
•NO-WAIT') 
■ NOR E ALT I ME") 
•NO-REALTIME" ) 
•NONREALTIME" ) 
•NO- PAUSE") 
•NO PAUSE" ) 


=  =  0) 


{ 


irl  St 


if  (DISPLAYSCREEN;  printf 
REALTIME  =  FALSE; 


)  ) 
"  [%s] \n" ,  keyword) 


if 


(strcmp  (keyword, 

(strcmp  (keyword, 

(Strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword, 

(strcmp  (keyword. 


•QUIT") 

•  STOP " ) 

•DONE") 

•EXIT") 

•REPEAT") 

•RESTART" ; 

•COMPLETE' 


0) 
0) 
0) 
0) 
0) 
0) 
0) 


■KILL")      ==  0) 
"SHUTDOWJ")  ==  0) 


I  I 


/» 
if 


} 


note  this  command  does  not  reset  LOOPFOREVER,  except  for    */ 
KILL/SHUTDOWN  which  terminate  the  dynamics  model  connection  • / 
' (Strcmp  (keyword,   "KILL")      ==0)  II 
(strcmp  (keyword,   " SHUTDOWvi " )  ==  0)) 


LOOPFOREVER  =  FALSE; 

strcpy  (buffer,  "KILL"); 

send_data_on_virtual_world_socket 


();  /•  buffer  msg  sent  •/ 


printf  (•[%s]\n",  keyword); 
if  (DISPLAYSCREEN)  printf  (' 
end_test  =  TRUE; 

read_another_line  =  FALSE; 


(end_test    set   TRUE]\n'); 


fclose    (auvscriptf ile) ; 
auvscriptf ilequit   =   TRUE; 
if     (DISPLAYSCREEN) 
print f (■ (QUIT  condition:     (%s   backup   file)    mission. script .backup,    file   closed] \n' 

AUVSCRIPTFILENAME) ; 
fprintf    (auvordersf ile, 
i6.1f    %5.1f    %5.1f    %5.1f    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 


■%6.1f       %< 
%5.1f \n". 


x_dot 
y_dot 
z   dot 


t,    psi_command,    x_coiranand,   y_coiimand,    z_coiTimand, 

port_rpm_command,    stbd_rpm_coitmiand, 

rudder_command,        planes_coiTunand, 

bow_lateral_thruster_command, 

stern_lateral_thruster_command, 

bow_vertical_thruster_command, 

stern_vertical_thruster_command) ; 

=  0.0; 


=  0. 
=  0, 
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phi_doc  =  0.0;    /*   degrees/sec    */ 

theta_dot  =  0.0;    /•   degrees/sec    */ 

psi_dot  =  0.0;    /*   degrees/sec    •/ 

speed  =  0.0; 

u  =0.0; 

V  =0.0; 

W  =  0.0; 

p  =  0.0;    /*    degrees/sec    */ 

q  =  0.0;    /•   degrees/sec    */ 

r  =  0.0;    /•   degrees /sec    */ 

delta_pianes  =  0.0;    /*    degrees             */ 

delLci_rudder  =  0.0;    /*   degrees             */ 

port_rpm  =  0; 

stbd_rpm  =  0; 

vertical_chruscer_volts  =  0.0; 

lateral_thruster_volts  =  0.0; 

else        if    i  iscrcmp  (keyword,  "RPM")  ==0)     II 

(scrciTip  (keyword,  "SPEED")  ==0)     II 

i.-trcmp  (keyword,  "PROPS")  ==0)     II 

istrcmp  (keyword,  " PROPELLORS" )  ==    0)) 

paraiTiecers_read   =   sscanf    (coiranand_buf  f  er ,    •%s%lf%lf", 

keyword,  &   parameterl , 

U   paraineter2  )  ; 
if       (paraineters_read   ==    3) 
{ 

printf     {"[%s        %6.2f    %6.2f]\n",  keyword,  parameterl, 

paramecer2 ) ; 
port_rpiri_command        =   parameterl; 
stbd_rpiri_comrTiand        =   parameter2; 
) 

el  se 
f 

parameters_read  =  sscanf  (cominand_buf  fer ,  '%s%lf", 

ke>'vord,  &  parameterl); 
printf  '"[%s      %6.2f!\n",  keyword,  parameterl); 
if   (parameters_read  ==  2) 
i 

port_rpm_command   =  parameterl; 
stbd_rpm_command   =  parameterl; 
) 
else  printf  ("  warning:  no  value,  ignoredXn"); 

) 
) 

else    if  ( (strcmp  (keyword,   "COURSE")   ==0)  II 
(strcmp  (keyword,   "HEADING")  ==0)  II 
(strcmp  (keyword,   "YAW")     ==0)) 
( 

parameter3_read  =  sscanf  (command_buf f er ,  •%s%lf", 

keyword,  &  parameterl); 
printf  ("[%s   %6.2f]\n",  keyword,  parameterl); 
if   (parameters_read  ==  2) 
{ 

DEADSTICKRUDDER  =  FALSE; 
WAYPOINTCONTROL  =  FALSE; 
psi_command     =  parameterl; 

rotate_command  =  0.0; 
lateral_command  =  0.0; 
ROTATECONTROL    =  FALSE; 
LATERALCONTROL   =  FALSE; 

) 

else  printf  (*  warning:  no  value,  ignoredXn'); 
) 

else    if  ((strcmp  (keyword,   "TURN")  ==0)  || 

(strcmp  (keyword,   •CHANGE-COURSE")  ==0)) 
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paramecers_read  =  sscanf  ( command_buf f er ,  *%s%lf", 

keyword,  &  parameterl ) 
pr^nrf  ("1%?     %6.2f)\n",  keyword,  parameterl ) ; 
if   (paran"ieters_read  ==  2; 
i 

DEADSTICKRUDDER  =  FALSE; 

WAYPCINTCONTROL  =  FALSE; 

ROTATECONTROL    =  FALSE; 

psi_command     =  psi_cominand  -►  parameterl ; 
) 

else  princf  ("  warnina:  no  value,  ignored\n" ) ; 
) 

else    if  (strcmp  (keyword,   "RUDDER")  ==  0) 

1 

paran"ieters_read  =  sscanf  (coinmand_buf  f  er ,  "Isllf", 

keyword,  &  parameterl) 
printf  ('[Is    %6.2f]\n",  keyword,  parameterl) ; 
if   (para]Tiecers_read  ==  2) 
I 

DE/i.DSTICKRUDDEF  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL    =  FALSE; 

HOVERCONTROL     =  FALSE, 

rudder_corrariand     =   parameterl; 
\ 

el  se 

printf  ("  warning:  improper  value,  rudder  order  ignored\n") 
"i 

else    if  (strcmp  (keyword,   "DEADSTICKRUDDER")  ==  0) 

parar'.eterr_re3d  =  sscanf  (  command_buf  f  er  ,  "%s%lf", 

keyword,  &  parameterl) 
if   vpara:T.eters_read  ==  2) 
1 

printf  ("!%?   %6.2fi\n",  keyword,  parameterl); 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL    =  FALSE; 

HOVERCONTROL     =  FALSE; 

rudder_commiand   =  parameterl; 


printf  ("l%s]   ",  keyword); 
DEADSTICKRUDDER  =  TRUE; 
WAYPOINTCONTROL  =  FALSE; 
ROTATECONTROL    =  FALSE; 
r\}dder_command  =  0.0; 

printf  ("  warning:  improper  value,  rudder  set  to  0\n"); 
} 
) 

else   if   (strcmp  (keyword,   "DEPTH")  ==  0) 
{ 

parameters_read  =  sscanf  (command_buf fer,  •%s%lf", 

keyword,  &  parameterl) 
printf  (•[%s    %6.2f)\n",  keyword,  parameterl); 
if   (parameters_read  ==  2) 

{ 

DEADSTICK PLANES  =  FALSE; 

z_command  =  parameterl; 
} 
else  printf  ("  warning:  no  value,  ignoredXn"); 

} 

else   if  (strcmp  (keyword,   "PLANES")  ==  0) 
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parameters_read  =  sscanf  (command_buf f er ,  •%s%lf", 

keyword,  &  paramecerl); 
printf  ("1%£   %6.2f]\n",  keyword,  parameterl ) ; 
if   !parameLers_read  ==  2) 
{ 

DEADETICK PLANES  =  TRUE; 
plane£_comniand  =  parameterl; 
} 

else  prinif  ("  warning:  improper  value,  planes  order  ignored\n" ) ; 
) 

else    if  (strcmp  (keyword,   'DEADSTICKPLANES" )  ==  0) 
{ 

paraiTieter?_read  =   sscanf    (command_buf f er,    "%s%lf", 

keyword,    &  parameterl) ; 
if      fparaiTiecers_read   ==   2) 


( 


printf    ("('oS        %6.2f]\n",    keyword,    pararoeterl)  ; 
DEADSTICKPLANES    =    TRUE; 
planes_CGmmand      =   pararrieterl ; 


printf  I'l^s]   ",  keyword); 

DEADSTICKPLA:CES  ^  TRUE; 

planes_comn-iand  =  0.0; 

printf  {"  warning:  improper  value,  planes  set  to  0\n"); 


} 


I 

else        if     Mstrcmp  (keyword,  "THRUSTERS-ON" )    ==0)     II 

(strcmp  (keyword,  "THRUSTERS")  ==0)     II 

(strcmp  (keyword,  'THRUSTERON" )    ==0)  II 

(Strcmp  (keyword,  'THRUSTERSON" )   ==  0)) 
{ 

printf  ("[%s]\n",  keyword); 
THRUSTERCOIJTROL  =  TRUE; 
) 

else    if   (Strcmp  (keyword,  "NOTHRUSTER" )     ==0)  II 

(strcmp  (keyword,  "NOTHRUSTERS" )    ==0)  II 

(strcmp  (keyword,  "THRUSTERS-OFF" )  ==0)  II 

(strcmp  (keyword,  "THRUSTERSOFF"  /   ==  Om 

printf  ("i%si\n",  keyword); 
THRUSTERCONTROL  =  FALSE; 
) 

else    if  istrcmp  (keyword,   "ROTATE")     ==  0) 
i 

parameters_read  =  sscanf  (command_buf f er ,  "Isllf", 

keyword,  &  parameterl) ; 
printf  ("i%s   %6.2fj\n",  keyword,  parameterl); 
if   (parameters_read  ==  2) 
i 

THRUSTERCONTROL  =  TRUE; 
WAYPOINTCONTROL  =  FALSE; 
HOVERCONTROL     =  FALSE; 

rotate_command  =  parameterl; 
clamp  (&rotate_command,  -12.0,  12.0,  "rotate_coiiimand" )  ; 
lateral_command  =  0.0; 
ROTATECONTROL    =  TRUE; 
LATERALCONTROL   =  FALSE; 

) 

else  printf  ('  warning:  no  value,  ignoredXn"); 

) 

else   if  ({strcmp  (keyword,  •NOROTATE')  ==0)  II 

(strcmp  {keyword,  "ROTATEOFF")  ==0)  I  I 

(strcmp  {keyword,  •ROTATE-OFF")  ==  0)) 
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princf  ("[%s]\n",  keyword); 
roLare_cominand  =  0.0; 
ROTATECONTP.CL   =  FALSE; 
} 

else        if  isrrcmp  (keyword,   "LATERAL")     ==  0) 
i 

paramecers_read  =  sscanf  (command_buf f er ,  "%s%lf", 

keyword,  &  parameterl) ; 
printf  ("|%s   %6.2f)\n",  keyword,  parameterl ) ; 
if   (parainecer£_read  ==  2) 
( 

THRUSTERCONTROL  =  TRUE; 
WAYPGINTCONTROL  =  FALSE; 
HOVERCONTROL     =  FALSE; 

rctate_cominand  =  0.0; 
lateral_coininand  =  parameterl; 

clamp  (i  lateral_command,  -0.5,  0.5,  •lateral_comm5rid" )  ; 
ROTATECONTROL    =  FALSE; 
LATEFoALCONTROL   =  TRUE; 
) 

else  printf  ('  warning:  nc  value,  ignoredXn"); 
) 

else  if  !  istrcmp  (keyword,  "NOLATERAL")  ==0)  II 
■.strcmp  (keyword,  "  LATERALOFF " )  ==0)  II 
'Strcmp   keyword,   • LATERAL- OFF " )   ==  0)) 

printf  (  "  i  % p  ;  \  n " ,  ke^-wor d  i  ; 
lateral_comiTiand  =  0.0; 
LATERALCONTRCL   =  FALSE; 

else   if  I,  (Strcmp  (keyword,   "POSITION")     ==0)  II 
(Strcmp  (keyword,   "LOCATION")     ==  0)  ) 

para-eter£_read  =  sscanf  (comm.and_buf  f  er ,  •%s%lf  %lf  %lf  " , 

keyword,     &  parameterl, 

&  parameter2,  &  parameters) 

printf  ("lis   %e.2f  %6.2f  %6.2f]\n",    keyword,      parameterl, 

parametfer2,   parameters) 
if   iparameters_read  ==  4; 

X  =  parameter!; 
y  =  parameter2; 
2  =  parameter^; 
/'  skip  line  in  telemetry  file  to  break  point-to-point  lines  */ 
fprintf  (auvdataf ile,  "\n"); 
} 

else  printf  ("  warning:  invalid  x/y/z  position,  ignoredXn"); 
) 

els<^   if  ((Strcmp  (keyword,   "ORIENTATION")  ==0)  || 
(strcmp  (keyword,   "ROTATION")     ==  0)  ) 

{ 

parameters_read  =  sscanf  (command_buf f er ,  ■%s%lf %lf %lf" , 

keyword,    &  parameterl, 

&  parameter2,  &  parameters) 

printf  (•l%s   %6.2f  %6.2f  %6.2f]\n",    keyword,      parameterl, 

parameter2,    parameters) 
if   (parameters_read  ==  4) 
{ 

phi   =  parameterl; 
theta  =  parameter2; 
psi    =  parameters; 
} 

else 
printf  (•  warning:  invalid  phi /theta/psi  orientation,  ignoredXn") 

) 

else   if  ((strcmp  (keyword,   "CONTINUE")  ==0)  II 
(strcmp  (keyword,        "GO")  ==0)) 
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if  (DISPLAYSCREEN)  printf  (•[%s]",  keyword); 

return;  /•  no  action  required  */ 
) 
else   if  ( (strcmp  (keyword,        "STEP")  ==0)  II 

(strcmp  (keyword,  "SINGLE-STEP")  ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("l%s]",  keyword); 

time_next_coiTiniand   =    t    ->■   dt ; 

read_another_line  =  FALSE; 
) 
else   if  ((strcmp  (keyword,   "TRACE")    ==0)  M 

(strcmp  (keyword,   "TRACE-ON")  ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("(TRACE  =  TRUE]  " ) ; 

TRACE  =  TRUE; 
) 

else  if  ((Strcmp  (keyword,   "TRACEOFF")   ==0)  II 
(strcmp  (keyword,   "TRACE-OFF")  ==0)  II 
(strcmp  (keyword,   "NOTRACE" )    ==0)  II 
(Strcmp  (keyword,   "NO-TRACE")   ==0)) 
{ 

if  (DISPLAYSCREEN)  printf  (" [TRACE  =  FALSE]  "); 

TRACE  =  FALSE; 
) 
else  if  '(strcmp  (keyword,  "LOOPFOREVEF" )   ==0)  M 

(Strcmp  (keyword,  " LOOP- FOREVER " )  ==0)) 
i 

if  (DISPLAYSCREEN)  printf  ( " [LOOPFOREVER ]  "); 

LOOPFOREVER  =  TRUE; 
} 
else  if  ((strcmp  (keyword,   "LOOPONCE")   ==0)  II 

(strcmp  (keyword,   "LOOP-ONCE")  ==0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("[LOOPONCE]  "); 

LOOPFOREVER  =  FALSE; 
) 

else  if  ((strcmp  (keyword,   "LOOPFILEBACKUP" )  ==0)  II 
(strcmp  (keyword,   "LOOP-FILE-BACKUP" )  ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("[LOOPFILEBACKUP]  "); 

LOOPFILEBACKUP  =  TRUE; 
) 
else  if  ((Strcmp  (keyword,   "ENTERCONTROLCONSTANTS" )    ==0)  II 

(strcmp  (keyword,   " ENTER -CONTROL- CONSTANTS " )  ==0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("[ENTERCONTROLCONSTANTS]  "); 

ENTERCONTROLCONSTANTS  =  TRUE; 

get_control_constants  (); 

f flush  (stdin) ; 
) 
else   if  ((Strcmp  (keyword,   "SLIDINGMODECOURSE" )    ==0)  II 

(Strcmp  (keyword,   "SLIDING-MODE-COURSE" )  ==  0)) 
{ 

printf  ("[%s  =  TRUE]\n",  keyword); 

SLIDINGMODECOURSE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL    =  FALSE; 

HOVERCONTROL     =  FALSE; 
) 

else   if  ((strcmp  (keyword,   'SLIDINGMODEOFF" )    ==0)  II 
(strcmp  (keyword,   'SLIDING-MODE-OFF" )  ==  0)) 

{ 

printf  {"(%s:   SLIDINGMODECOURSE  =  FALSE] \n",  keyword); 
SLIDINGMODECOURSE  =  FALSE; 

) 

else   if  ((strcmp  (keyword,   •TACTICAL")    ==0)  II 
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e  1  se 


(strcmp  (keyword,   'TACTICAL-ON' )  ==  0)) 

printf  (■;%sj\n",  keyword); 
TACTICAL  =  TRUE; 

if  ((Strcmp  (keyword,   "NO-TACTICAL")   ==  0) 
(Strcmp  (keyword,   "TACTICAL-OFF" )  ==  0) 


printf  (' 
TACTICAL 


%s]\n- , 
:  FALSE; 


keyword) 


else 
( 


if  (strcmp  (keyword. 


if  (DISPLAYSCREEN)  printf 
SONARTRACE  =  TRUE; 


SONARTRACE')    ==    0) 
[SONARTRACE] 


else 
i 


ei  se 


if  (Strcmp  (keyword,   "SONARTRACEOFF" )  ==  0) 

if  (DISPLAYSCREEN!  printf  ( "1 SONARTRACEOFF ]  '); 
SONARTFJiCE  =  FALSE; 

if  (Strcmp  (keyword,   " SONAR INSTALLED ' )  ==  0) 


if  (DISPLAYSCREEN)  printf 
SONAR INSTALLED  =  TRUE; 


[SONARINSTALLED]  ') 


else  if  (Strcmp  (keyword,   • PARALLELPORTTRACE ■ )  ==  0) 

if  (DISPLAYSCREEN,  printf  ( " [ PARALLELPORTTRACE ; 
PAPuE-.LLELPORTTRACE  =  TRUE; 


r-se 


(Strcmp 

(keyword. 

■AUDIBLE- ) 

=  = 

0)  1  1 

(Strcmp 

(keyword. 

■AUDIO') 

=  = 

0)  1  i 

i strcmp 

I  keyword. 

■AUDIO- ON") 

=  = 

0;   1  1 

(strcmp 

(keyword, 

■SOUND- ON") 

=  = 

0)   1  1 

(strcmp 

(keyword. 

• SOUNDON " ) 

=  = 

0)   i  1 

(Strcmp 

[keyword, 

■SOUND" i 

=  = 

0)  ) 

if  ^LISFLAYSCREEK'  printf  '" [AUDIBLE]  "); 

strcpy  (buffer,  "AUDIBLE");  /*  copy  current  comir.and  to  buffer  */ 

se:'id_data_orj_virtuaI_world_socket  ();  f   send  to  sound  driver  */ 

else  if  ((strcmp  (keyword,  "SILENT")  ==  0)  I 
(strcm.p  (keyword,  "SILENCE")  ==  0)  I 
i strcmp  (keyword,  "NOSOUND")  ==0)  I 
(strcmp  (keyword,  'SOUNDOFF^)  ==  0)  I 
(Strcmp  (keyword,  ■SOUND-OFF")  ==  0)  i 
(Strcmp  (keyword,  "AUDIOOFF')  ==  0)  I 
(strcmp  (keyword,  "AUDIO-OFF^)  ==  0)  I 
(Strcmp  (keyword,  "QUIET";  ==  0)) 
{ 

if  (DISPLAYSCREEN)  printf  ("[SILENT]  "); 

strcpy  (buffer,  "SILENT");   / "  copy  current  command  to  buffer  */ 
send_data_on_virtual_world_socket  ();  /•  send  to  sound  driver  •/ 
) 

else  if  ( (strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 

( 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ('[EMAIL  ON]  ') 
EMAIL  =  TRUE; 


•EMAIL') 

=  = 

0) 

•EMAIL-ON") 

=  = 

0) 

•E-MAIL") 

=  = 

0) 

•E-MAIL-ON") 

=  = 

0) 

"EMAILON") 

=  = 

0)  ) 

) 

else  if  ((strcmp  (keyword,  "EMAILOFF")  ==0)  II 

(strcmp  (keyword,  'EMAIL-OFF")  ==0)  II 

(strcmp  (keyword,  "E-MAILOFF")  ==0)  II 
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} 

el 


") 


(strcmp  (keyword,  'E-MAIL-OFF" )  ==0)  II 

(scrcmp  (keyword,  "NO-E-MAIL")  ==0)  II 

(strcmp  (keyword,  "NO-EMAIL")  ==0)  II 

(strcmp  (keyword,  "NO-E-MAIL")  ==0)  II 

(strcmp  (keyword,  "NOEMAIL")  ==0)) 

if  (TRACE  &i.  DISPLAYSCREEN)  printf  ("  [EMAIL  OFF] 
EMAIL  =  FALSE; 

se  if  ((strcmp  (keyword,  "WAYPOINT" )    ==0)  II 
(strcmp  (keyword,  "WAYPOINT-ON" )  ==  0)) 

parameters_read  =  sscanf  (command_buf f er ,  •%s%lf %lf %lf  " , 

keyword,  &  parameterl, 

&  paraiiieter2 ,    &  parameters) 
(parameters_read  ==   4) 


if 

i 


printf  {"l%s   %6.2f  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter2,  parameters) 
WAYPOINTCONTROL  =  TRUE; 
x_command       =  parameterl 


y_command       =  parameter2, 

2_command       =  parameters, 
port_rprTi_command       =  fabs  (port_rpm_command) 
stbd_rpm_comm.and       =  fabs  (stbd_rpm_command) 

fprintf  (auvordersf : le. 


/*  ensure  fwd 
/*  motion  only 


t,  ps:_command,  x_command,  y_command,  2_command, 

port_rpm_command,  stbd_rpm_command, 

rudder_command,    planes_command, 

bow_lateral_thruster_command, 

stern_lateral_thruster_command, 

bow_vertical_thruster_command, 

sterri_vertical_thruster_command)  ; 


) 

else  if   iparameters_read 


printf 


%6.2f  %6.2f ] \n" 


keyword,  parameterl, 
parameter2 ) 


if   %6. 


WAYPOINTCONTROL  =  TRUE; 
x_command       =  parameterl; 
y_command       =  parameter2; 
port_rpm_corrdDand       =  fabs  (port_rpm_command)  ;  /•  ensure  fwd 
stbd_rpiT,_command       =  fabs  (stbd_rpm_command)  ;  /*  motion  only 
fprintf  (auvordersf lie. 
If  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f   %5.1f  %5.1f  %5.1f 


t,  psi_command,  x_command,  y_command,  z_command, 

port_rpm_command,  stbd_rpm_command, 

rudder_command,    planes_command, 

bow_lateral_thruster_command, 

stern_lateral_thruster_command, 

bow_vertical_thruster_command, 

stern_vertical_thruster_command) ; 


else 
{ 


•%6. 

%5.1 


If   %6 

f\n". 


fpri 
If  %5.1f 


WAYPOINTCONTROL  =  FALSE; 

printf  ( • \n  warning:  improper  number  of  values\n  waypoint"); 
printf  ("set  to  current  position  but  otherwise  ignoredXn'); 
x_command       =  x; 
y_command       =  y; 
z_command       =  z; 
ntf  (auvordersf ile, 
%5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f   %5.1f  %5.1f  %5.1f 
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t,    psi_cominand,    x_command,    y_coiTiinand,    z_cornmand, 
port_rpiri_comiTiand,    stbd_rpin_command, 
rudder_comrriand,        planes_cominand, 
bow_lai:eral_thruster_cominand, 
stern_lateral_chruscer_command, 
bow_vercical_thruscer_command, 
stern_vertical_thruscer_coinmand)  ; 
] 

if   (FOLLOWWAYPOINTMODE  ==  TRUE) 
{ 

it     (TRACE)  printf  {• [FOLLOWWAYPOINTMODE  check] ') ; 
/•  continue  until  WAYPOINT  reached  without  further  script  orders 
tiine_next_coiTunand  =  t  +  2.0  •  dt  ; 
i'ead_another_Iine  =  FALSE; 
} 
) 
else   if  ( (strcmp  (keyword,   "WAYPOINTFOLLOW )    ==0)  II 

(strcmp  (keyword,   "WAYPOINT-FOLLOW )  ==  0)  ) 
f 

prmtf  i"[%sl\n",  keyword); 
FOLLOWWAYPOINTMODE  =  TRUE; 
1 
else    If  (  ;scrcmp  (keyword,   -WAYPOINTFOLLOWOFF" )     ==0)  II 

(Strcmp  (keyword,   'WAYPOINT-FOLLOW-OFF" )  ==  0)  ) 
{ 

print f  ("i%s)\n',  keyword. i  ; 
FOLLOWWAYPOINTMODE  =  FALSE; 
/ 

else  it     (  (strcir-r-  (keyword,  "STANDOFF")  ==0)  II 

(Strcmp  (keyword,  "STANr-OFF";  ==0)  II 

{strcmp  (keyword,  ■ STANDOFFD I STANCE " )  ==0)  M 
(strcmp  (keyword,  " STANDOFF- DI STANCE " )  ==0)  II 
(strcmp  (keyword,  " STAND- OFF -DI STANCE " )  ==  0)) 

paramcters_read  -    sscanf  (command_buf f er ,  •%s%lf* , 

keyword,     it   parameterl 
if   (parameters_read  ==  2) 

prmtf  i"l%s    %6.2f]\n",  keyword,  parameterl); 
standcf f_distance  =  parameterl; 


prmtf  ("[%3;\n",  keyword); 

prmtf  ("  warning:  no  standoff  value  provided,  ignored"); 
} 
) 
else  if  ((strcmp  (keyword,  "HOVER")     ==0)  II 

(strcmp  (keyword,  " HOVER- ON " )  ==  0)) 
{ 

parameters_read  =  sscanf  (coinmand_buf  f  er ,  "%s%lf  %lf  %lf  %lf  %lf  " , 

keyword,    &  parcuneterl , 
&i  parameter2,  &  paranieter3, 
&  paraineter4,  &  parameters; 
if   (paraineters_read  ==  6) 


{ 


printf  (■(%s   %6.2f  %6.2f  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl, 
parameter2,  parameter3, 
parameter4,  parameters); 

HOVERCONTROL      =  TRUE; 

WAYPOINTCONTROL    =  FALSE; 

ROTATECONTROL      =  FALSE; 

THRUSTERCONTROL    =  TRUE; 

DEADST I CK RUDDER    =  TRUE; 

FOLLOWWAYPOINTMODE  =  FALSE; 
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"%6.1f       % 
%5.  If  \n", 


rudder_cominand 
x_comjnand 
y_command 
z_c  omnia  nd 
psi_comiTiand 

psi_coiTmand_hover   =   paraineter4  ; 
standof  f_distance   =   paraineter5; 
fprintf    (auvordersf ile, 
6. If    %5.1f    %5.1f    %5.1f    %6.1f    %6.1f    %6.1f    %6.1f 


=  0.0; 

=  pararrieterl  ; 

=  paranieter2  ; 

=  parameters,- 

=  paramieter4  ; 


%5.1f    %5.1f    %5.1f 


t,  psi_command,  x_command,  y_command,  z_command, 

port_rpm_command,    stbd_rpm_coinmand, 

rudder_command,    planes_command, 

bow_lateral_thruster_command, 

stern_iateral_thruster_command, 

bow_vertical_thruster_command, 

stern  vertical  thruster  command) ; 


else  if   (parameters_read  ==  5) 


p  r  I  n  t  f 


;%s 


%6.2f  %6.: 


f  %6.2f  %6.2f]\n" 
keyword 


parameterl , 
parameter2,  parameters, 
parameter4 ) ; 


HOVERCOKTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

RCTATECONTROL  =  FALSE; 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 
FOLLOWWAYPOINTMODE  =  FALSE; 

rudder_command  =  0.0; 

x_command  =  parameterl 

y_command  =  parameter2 

z_commarid  =  parameters 

psi_command  =  parameter^ 

psi_command_hover  =  parameter^ 
fprintf  (auvordersf ile, 

If  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f 


%5.1f  %5.1f  %5.1f 


\n 


t,  psi_command,  x_command,  y_command,  z_command, 

po  r t_r piTi_c  omma nd ,    s  t  bd_rpm_c  omma  nd , 

rudder_comiriand,        planes_command, 

bow_lateral_thruster_command, 

stern_lateral_thruster_command, 

bow_vertical_thruster_command, 

stern_vertical_thnJster_command) ; 


else  if 


(parameters_read  -- 
printf  ("  l%s    %6.2f  %5 


HOVERCONTROL 


=  TRUE; 


2f  %6.2f]\n",  keyword,  parameterl, 
parameter2,  parameters) 


•%6  .1 
%5.1f 


f   %6 

\n", 


WAYPOINTCONTROL  =  FALSE; 
ROTATECONTROL    =  FALSE; 
THRUSTERCONTROL  =  TRUE; 
DEADSTICKRUDDER  =  TRUE; 
FOLLOWWAYPOINTMODE  =  FALSE; 
rudder_command  =  0.0; 
x_command       =  parameterl; 
y_command       =  parameter2; 
z_command       =  parameters; 
fprintf  (auvordersfile. 
If  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f 


%5.1f    %5.1f    %5.1f 


t,    psi_command,    x_conimand,   y_command,    z_cornmand, 
port_rprri_command,    stbd_rpm_command, 
rudder_command,        planes_command. 
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bow_lateral_thruster_comiTiand, 
st€rn_laceral_chruster_coiTOTand, 
bow_verLical_thruster_cominand, 
scern_vertacal_thruster_cominand)  ; 
) 

else  if   (paramecers_read  ==  3) 
{ 

printf     (•[%£        %6.2f    %6.2f]\n",    keyword,    parameterl , 

parainecer2 )  ; 
HOVERCONTROL  =    TRUE; 

WAYPOINTCONTROL    =    FALSE; 
ROTATECONTROL         =    FALSE; 
THRUSTERCONTROL    =    TRUE; 
DEADST I CK RUDDER    =    TRUE; 
FOLLOWWAYPOINTMODE    =    FALSE; 
rudder_comrriand      =    0.0; 
x_command  =   parameterl ; 

y_comiriand  =   paraineter2; 

fprintf     (auvordersf i le, 
•'cfc.lf      %c.lf    lE.lf    %5.1f    %5.1f    %6.1f    %5.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 
%E.  .if  viV, 

t,    p3i_cominand,    x_cominand,    y_cominand,    2_command, 
porc_rpiTi_cominand ,    stbd_rpin_cominand, 
rudder_corrmiand,        planes_command, 
bow_lat:eral_thruster_cominand, 
stern_lateral_thruster_command, 
bow_vercical_thruster_command, 
sLern_vertical_chruster_cominand)  ; 
) 

else   if      (parameters_read  ==    1) 
{ 

printf    (•[%s]\n",    keyword); 
HOVERCONTROL  =    TRUE; 

WAYPOINTCONTROL    =    FALSE; 
ROTATECONTROL         =    FALSE; 
THRUSTERCONTROL    =    TRUE; 
DEADETICKRUDDER    =    TRUE; 
FOLLOWWAYPOINTMODE    =    FALSE; 
ruddfer_cominand      =    0.0; 
fprincf     (auvordersf ile, 
•■s^.:f      ".e.if    hi. It    hS.lt    %E.lf    %6.1f    %6.1f    %6.1f    %6.1f      %5.1f    %5.1f    %5.1f 
%L. i:  ,n", 

t,    psj_cominand,    x_command,    y_cominand,    2_command, 
porL_rpiTi_comiTiand,    st:bd_rpm_cominand, 
rudder_comTnand,        planes_comniand, 
bow_laceral_thruster_cominand, 
stern_lateral_thruster_conunand, 
bow_vertical_thruster_coiranand, 
scern_verLical_thruster_command) ; 
) 

else 
{ 

printf    (•[%s]\n",    keyword); 

printf    (•   warning:    improper  number   of  values,    ignoredXn"); 
) 
) 

else        parse_mission_string_corranands    (coiTiinand_buf  fer)  ; 
/»   check   other  possibilities   •/ 

if    (audible_cominand  ==  TRUE) 

{ 

strcpy    (buffer,    command_buffer) ;    /*   copy  current   coiranand  to  buffer  */ 
send_data_on_virtual_world_socket    ();  /*   send   to  sound  driver   */ 

) 

if  ( (print_help  ==  TRUE)  &&  DISPLAYSCREEN) 
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princf  ("%s",  coiTiinand_buf  f  er  )  ; 
princ_va2 jd_keywords  (  ;  ; 

strcpy  (buffer,  "  is  an  unknown  command");  /*  copy  msg  to  buffer   */ 
send_dat3_on_vircual_worid_socket  ( ) ;      /*  send  to  sound  driver  */ 

) 

princ_help  =  FALSE;  /*  reset  value  */ 

)  /•  loop  until  read_another_line  is  FALSE)  */ 

if  [TRACE  UL    DISPLAYSCREEN) 

printf  ("[end  parse_mission_script_commands  ()]\n"); 

return; 

}    /*   end  parse_miss:on_script_cominands    ()    •/ 

vn:;  pai's^_rTiis3ion_st ring_coirjr,ands    (command) 
cnar    *    command; 

int     index; 

inr    number_values  =  0; 

char   parameter2  [60]; 

:f  'TRACE  &&  riSPLAYSCREE'Ji 

pi'intf  ■'  "  \r;  ;parse_mission_string_commands  start  ]  \n" )  ; 

r.-T.Ler_valu^:-   =    sscanf    (corpjnand_buf fer ,    "%s",    keyword); 

for  (index  =  U;  index  <=  (mt;  strlen  (keyword);  index*+) 
keyword  [index]  =  toupper  (keyword  [index]); 

if      (nuiT,ber_values  !=  1; 

if  (LISFLAYSCREENi  prmtf  ("  [no  parse  word  found]  \n"); 
returr. ; 

if  { istrcmp  (keyword,   "REMCTEHOST" )   ==0)  II 
(strcmp  (keyword,   "REMOTE")      ==0)  II 
(strcmp  (keyword,   ' REMOTE- HOST " )  ==0)  II 
istrcmp  (keyword,   "HOST" j        =-   0)) 
{ 

if  (sscanf  (command,  "%s  %s',  keyword,  parameter2)  ==  2) 
1 

strcpy  (virtual_world_remote_host_name,  parameter2); 
if  (DISPLAYSCREEN;   printf  (" [REMOTE-HOST  %s]  ", 

virtual_world_remote_host_name) ; 
) 

else  print_help  =  TRUE; 
) 
else  print_help  =  TRUE;   /*  invalid  command  line  entry  parameter  found  */ 

if  (TRACE  &£.  DISPLAYSCREEN) 

printf  ( • \n lparse_mission_string_commands  complete] \n' ) ; 

return; 

)/*  end  parse_mission_string_commands  ()  */ 

/*»♦*««*»♦«*«********«*«**********«♦******♦**««***«*««•«*♦***««**«♦«»♦««*«*«*»«/ 
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E.  globals.c  Global  Variable  Instantiation 


rTr*»T*»*«i 


r******«*»***«»1 


•««♦««♦««*«*»***♦»*»»«»»*»»«»« 


FrograiT:: 

Authors : 

Revised: 

£>>•  s  t  err. : 
Compiier : 

Compilation : 

168020] 
[66030] 


Irix  J 


Pui-pc?. 


globals.c 

Don  Brutzman 

21  October  94 

AUV  Gespac  68020/65030,  OS-9  version  2.4 
Gespac  cc   Kernighan  &  Richie  (K&R)  C 

ftp>     put  globals.c 
auvsiri:-,-  chd  execution 
auvsiml'-  make  -k.2f  execution 
auvsiml-  make      execution 

fletch>  make  execution 

Allows  repeated  use  of  global  variables  global. c  via  global. h 
in  order  to  prevent  compiler  warnings 


«t.»nciude  "defines. h' 


/*'** 

/*  Program  configuration  flags 


*/ 

l=trace  on,     0=trace  off  */ 

l=screen  on,     0=screen  off  */ 

l=graphics  lab,  0=actual  vehicle  •/ 

l=tactical  on,   0=tactical  off  •/ 

l=repeat  execution  indefinitely  */ 
l=backup  files  between  replications*/ 

l=trace  each  char  received  at  port  */ 

l=sonar  head  available  for  query  */ 

l=trace  on,     0=trace  off  */ 

l=manual  entry,  0=default  values  •/ 

1=1  second  real-time  waits,  O=none  */ 

l=dead  reckon  navigate,  0=regular  •/ 

l=use  ordered  rudder,  0  =  control  */ 

l=use  ordered  planes,  0  =  control  */ 

l=use  sliding  mode,    0  =  control  */ 

l=use  thrusters,  0=use  propellers  */ 

l=use  thrusters  to  rotate  in  place  */ 

l=use  thrusters  for  lateral  motion  */ 

1=    go  to  WAYPOINT  without  WAITs  */ 

1=   go  to  WAYPOINT  •/ 

l=hover  at  WAYPOINT  •/ 


int 

TRACE 

I  nt 

DliPU-.YSCREEN 

int 

LOCATIONLAB 

int 

TACTICAL 

i  ;■;: 

LCCPFOPEVEF 

:nt 

LOCPFILEBACKUP 

ii-.t 

PArJ^.LLELPORTTRACE 

int 

SONAR INSTALLED 

1 1"  t 

SOKARTRACE 

ir.r 

Er.'TERCONTRCLCONSTANTS 

int 

REALTIME 

int 

DEADRECKON 

int 

DEADSTICKRUDDER 

int 

DEADSTICKPLANES 

int 

SLIDINGMODECOURSE 

int 

THRUSTERCONTROL 

int 

ROTATECONTROL 

int 

LATERALCONTROL 

int 

FOLLOWWAYPOINTMODE 

int 

WAYPOINTCONTROL 

int 

HOVERCONTROL 

«if  de 

f inedfsgi ) 

int 

EMAIL 

telse 

int 

EMAIL 

#endif 

1; 
0; 


/• 
/■* 
/• 
/* 
/' 

/' 
/* 
/« 
/• 

/* 
/* 
/• 
/* 
/• 
/• 


/*   l=send  e-mail,  O=don't  send  e-mail  •/ 
/•  can't  send  e-mail  via  OS-9  directly  '/ 


int 


NOT  YET  REIMPLEWENTED 


=  0;    /•   code  in  block  needs  reverif ication  •/ 
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dc-ctle   TIKESTEP 


iTit  KEYBOAF.DINPUT 


;  '  t^les   and  paths 

FILE  '  auvscripcf iie; 

FIL-E  '  auvordersf  ile; 

FILE  *  auvdacafile; 

FILE  '  auvCext f iie; 

FILE    '  controicorijcantsf iie; 

FILE  *  emaiiaddressf iie; 


/*  FILE  *  serialtestf ile;   */ 
int   serialpath  =  0; 
int   sonarpath  =  0; 

,'  Variables  and  data  structures 


0.1;  Z'   time  of  a  single  closed  loop 

/"   add  code  to  warn  if  exceeded  <«< 


0; 


/*   l=read  keyboard  vice  mission  file 


*/ 


************ 


Duffers  of  full  strings  for  byte  transfer  to  tactical  level  &  disk  file 
'buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error 


t  ...Tie_t 
struct    tri' 


systerT,_tinie 
'3ysteri_tiTip 


=  U  ; 
=  0; 


"  partial  structure  template  for  the  MFI  (only  interested  in  PIA  for  now) 

;trucr  MFI  PIA 


1 


unsignea  snort  pra 
unsigned  short  era 
unsigned  short  prb 
-hsianed  short  citj 


!*  port  register  A  -  data  direction  A 

/*  control  register  A 

/■*  port  register  B  -  data  direction  B 

/*  control  register  B 


/*   4  Channels  of  DAC  ADA-1  DAC 

unsigned  char   *daci_a   =  lunsigned  char   *)  DAC1_ADDR; 

:■'      6  Channels  of  DAC  DAC-2B 

unsigned  char   'dac2b_a  =  (unsigned  char   *)  DAC2B_ADDR; 


/*  16  Channels  of  ADC  ADA-1 
unsigned  char   *adcl_a   =  (unsigned  char 


■)  ADC1_ADDR; 


/•  16  Channels  of  ADC  ADC- 2 

unsigned  short  *adc2_a   =  (unsigned  short  *)  ADC2_ADDR; 


unsigned  short  *viaO 


(unsigned  short  *)  VIAO_ADDR; 


mt 

int 

int 

int 

int 

double 

double 

double 

double 

double 

double 

int 

int 


telemetry_records_saved  =  0; 

inission_leg_counter  =  0; 

replication_count  =  1; 

end_test  =  FALSE; 

wrap_count  =  0; 

t  =  0.0; 

dt  =0.1, 

rpm  =  0.0; 

speed_limit  =  0.0; 

inain_motor_deltal  =  0.0; 

niaiR_motor_del  ta2  =  0.0; 

niain_inotor_voltl  =  512, 

main  motor  volt2  =  512, 
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double 

X 

= 

0.0 

double 

y 

= 

0.0 

double 

2 

= 

0.0 

double 

phi 

= 

0.0 

/• 

degrees 

*/ 

double 

theta 

= 

0.0 

/' 

degrees 

*/ 

double 

psi 

= 

0.0 

/• 

degrees 

V 

double 

x_dot 

= 

0.0 

double 

y_doc 

= 

0.0 

double 

z_doc 

= 

0.0 

double 

phi_doc 

= 

0.0 

/• 

degrees/sec 

*/ 

double 

theca_doc 

= 

0.0 

/• 

degrees/sec 

V 

double 

psi_doc 

= 

0.0 

/• 

degrees/sec 

V 

double 

speed 

= 

0.0 

double 

u 

= 

0.0 

doutle 

V 

X 

0.0 

double 

w 

= 

0.0 

double 

p 

= 

0.0 

/• 

degrees/sec 

•/ 

double 

q 

= 

0.0 

/* 

degrees/sec 

•  / 

double 

r- 

= 

0.0 

/* 

degrees/sec 

•/ 

double 

del ta_planes 

= 

0.0 

/* 

degrees 

•/ 

douLje 

delt:a_rudder 

= 

0.0 

/* 

degrees 

V 

double 

pcrt_rpm 

= 

0; 

double 

stbd_rpm 

= 

0; 

double 

verLical_thruster_volts 

= 

0.0 

dc„Lle 

laLeral_thruster_volts 

= 

0.0 

ur.sigrjed  snort   psi_bir_oid 

= 

0; 

dcuLle 

dg_of f sec 

= 

0.0 

double 

k_psi 

_ 

0.0 

dcucle 

k_r 

= 

0.0 

double 

k_v 

= 

0.0 

doutle 

k  z 

- 

0.0 

double 

k_w 

= 

0.0 

dcucle 

k  c  Tl  e  c  a 

= 

0.0 

douLle 

k_q 

= 

0.0 

k_rhruster_psi 

- 

0.0 

douH/le 

k_thrusLer_i' 

=. 

0.0 

double 

k_t:hrust;er_rocace 

= 

0.0 

doui:le 

k_chruscer_lateral 

= 

0.0 

double 

k_chruster_z 

=. 

0.0 

double 

k_thruster_w 

= 

0.0 

K_propfel lfer_hover 


0.0; 


QOUOie 


k_surge_hover 


=    0.0; 


dcuLle 

k_tr!ruster_hover 

= 

0.0; 

douLle 

k_sway_hover 

= 

CO; 

double 

k_sigma_r 

- 

12.0; 

double 

k_sigiTia_psi 

= 

28.87 

double 

eta_steering 

= 

0.1; 

double 

Sigma 

= 

0.0; 

int 


inission_legs_total 


=    0; 


values   initialized  in  parse_iTiission_script_conunands    ()    */ 


double 

time. 

_next_command       = 

0 

0; 

double 

psi_command       = 

0 

0; 

/* 

degrees 

•/ 

double 

psi_command_hover  = 

0 

0; 

/• 

degrees 

•/ 

double 

x_command       = 

0 

0; 

/* 

feet 

*/ 

double 

y_command       = 

0 

0; 

/• 

feet 

•/ 

double 

z_command       = 

0 

0; 

/* 

feet 

*/ 
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dooclc: 
double 
dr-,ubl^ 
dcabie 

double- 


stbd_rprTi_comir.and 

port_rpir,_coiTunand 

planes_comiTiand 

ruddei_comiT.ar.d 

rot.ace_cominand 

laceral    comjTiand 


0.0 
0.0 
0.0 
0.0 
0.0 
0.0 


/*  degrees  */ 

/*  degrees  ■•  / 

/*  degrees/sec  */ 

/•  ft/sec  «/ 


double  bow_lateral_thruster_cominand   =    0.0 

double  scern_iateral_thruster_comrriand   =    0.0 

double  bow_vert:ical_thruster_comiriand   =    0.0 

dcuLl<r  ste:-n_vertical_thruster_cominand  =    0.0 

douDle  waypoint_distance       =  0.0; 

double  waypoint_angle          =  0.0; 

double  track_angle            =   0.0 

double  along_track_distance    =   0.0 

double  cross_track_distance    =   0.0 

double  standoff  distance       =  10.0 


/*  volts  -24 . .24  */ 

/*  volts  -24 . .24  •/ 

/•  volts  -24 . .24  */ 

/♦  volts  -24. .24  */ 


doub] 

int 
int 

I  V.   L 

^nt 
int 
:n: 


double 
double 
double 
double 
dc^i.  le 
-r:t 


int 


int 
int 
int 
int 
mt 


depth_error ; 

roll_rate_G 

pi tch_rate_0 

yaw_rate_0 

roll_C 

pitch_0 

z_valO 

swi 

error 

range 

zjad_rng 

bad_jpda tes 

ranQe_inde>: 

range: 

rarjge2 

er rcrl 

errcr2 

avg_rng 

k_range 

range_array 

pointer 
speed_array 

PortAFlag 

tick 

curr_tick 

tickl 

tick2 

i 


JU  ju  J 


n 


=  NULL; 


=  0.0; 


int 

long 

long 

double 

short 

char 

int 

int 
int 
int 

int 


mask 

davedate 

davetime 

value 

day 

answer 

start_dwell 

socket_descriptor 

socket_accepted 

socket_streain 

socket_length 


OxOOOOffff ; 

0; 

0; 

0.0; 

0; 


=  0; 

=  0 
=  0 
=  0 


=  255;   /*  max  allowed  packet  size  */ 
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mt 
inc 
int 
inc 
int 

/'  char 

char 
int 
inc 
int 

char 


bytes_received 
bytes_read 
bytes_written 
bytes_lef t 
by tes_senc 


=  0 

=  0 

=  0 

=  0 

=  0 


buffer_array    1 FILEBUFFERSIZE] [256] 


not  implemented 


bu  f  f  e  r 

buf f er_saze 

buf f er_index 

variables_parsed 


[300 
=  0 

=  0 
=  0 


buf fer_received         [300], 
virtual_world_remote_host_name  [60 ; 
command  buffer  [3001; 


FILE  •    ne:rcat_f  ileptr.- 
struct  sockaddr_in      server_address ; 
?rrucr  ho? tent    *?erver_encity ; 

c.-.ar  keyword       [81  ]  ; 

c.-.a:  email  address  [81]; 


9hjtdowr,  s:anai  received 


=  FALSE; 


.  nt 


cha 


:n; 


dOUL-e- 


virtual_wcrld_socket_opened  =  FALSE; 


'  ptr_index; 


print_help 


=  FALSE; 


;peed_per_rpm  =  2.0  /  700.0 


steady  state: 

2.0  feet/sec  per  700  rpm  */ 


:ioCf-._t  previcuslocpclock 
r:ccK._t   currer.tloopclock 


=  0; 
=  0; 


;4V 


+-2  lb,  +  Volts  moves  thruster  in  +  direction,  all  identical  */ 


dcuL.^  AU\«'_bow_verticaI  =  O.G 

double  AL^_stern_vertical  =  0.0 

double  Al"v_bow_lateral  =  0.0 

double  ALV  stern  lateral  =  0.0 


/ *  thruster  rpm 

/*  thruster  rpm 

/*  thruster  rpm 

/*  thruster  rpm 


•/ 

•/ 


/•  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal 
double  Al.rv'_ST1000_bearing  =   0.0;/*  ST_1000  conical  pencil  bearing  •/ 
doubl<r  ALT_ST100C_range   =   10.0; /■•  ST_1000  conical  pencil  range   •/ 
double  A;.n_ST1000_3trength=   20.0;/*  ST_1000  conical  pencil  strength*/ 


double  A'Jv_ST7  25_bearing   = 
double  AUV_ST7  25_range 
double  A'JV_ST7  25_strength  = 


90.0;/*  ST_72  5 


20.0, 


ST  725 


int 


int 


audible_command 


10.0;/*  ST  725 


TRUE; 


1  X  24  sector  bearing  */ 
1  X  24  sector  range  •/ 
1  X  24  sector  strength  •/ 


auvscriptf ilequit   =  FALSE; 


r*****«*****V**1 


t»««»««»»*»»*««*»«»«««*»«««»»»*«*«»««*«»«»«««*«»»«»« 


99 


globals.h  Global  Variable  Define  File  to  Permit  Multiple  References 


Program: 

Autnors : 

Revised : 

Sy  s  t  em : 
Comp: 1 er : 

Compilation : 

[68020] 
168030) 

[Irix    ] 

Purpose: 


globals.h 

Don   Brutzman 

28   October   94 

AUV  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (Ki^R)  C 

ftp>     put  globals.h 
auvsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make      execution 

fletch>  make  execution 

Allows  repeated  use  of  global  variables  global. c  via  global. h 
in  order  to  prevent  compiler  warnings 


'*»»»«»**«*****««*«**«****i 


*v************ 


*.;icludtr  "defines. n' 


*-f  dt:.."ed  GLO&ALS_H) 

*cl  St 

♦define  GLOBAL£_H 

/•    Prograrr   conf igurarion    flags 


r»***«***1 


:******! 


r  «  «  *  *  *  1 


extern 

mt 

TRACE 

extern 

int 

DISPLAYSCREEN 

extern 

int 

LOCATIONLAB 

e  X  t  e  r  n 

.■::V 

TACTICAL 

exterr. 

,nt 

LOOPFOREVER 

extern 

:nt 

LOOPFILEBACKUP 

extern 

ant 

PARALLELPORTTRACE 

extern 

int 

SONAR  INSTALLED 

extern 

int 

SONAR TRACE 

extern 

int 

ENTERCONTRCLCONSTAJ 

extern 

int 

REALTIME 

extern 

int 

DEADRECKON 

extern 

int 

DEADSTICKRUDDER 

extern 

int 

DEADSTICK PLANES 

extern 

int 

SLIDINGMODECOURSE 

extern 

int 

THRUSTERCONTROL 

extern 

int 

ROTATECONTROL 

extern 

int 

LATERALCONTROL 

extern 

int 

FOLLOWWAY  PO I NTMODE 

extern 

int 

WAYPOINTCONTROL 

extern 

int 

HOVERCONTROL 

/• 

/•* 
/* 
/' 


l=trace  on, 
l=screen  on, 
l=graphics  lab, 
l^tactical  on. 


0=trace  off 
0=screen  off 
0=actual  vehicle 
0=tactical  off 


l=repeat  execution  indefinitely 


l=backup  files  between  replications*/ 

l=trace  each  char  received  at  port  */ 

l=sonar  head  available  for  query  */ 

l=trace  on,     0=trace  off  */ 

l=manual  entry,  0=default  values  */ 

1=1  second  real-time  waits,  O=none  */ 

l=dead  reckon  navigate,  0=regular  */ 

l=use  ordered  rudder,  0  =  control  •/ 

l=use  ordered  planes,  0  =  control  */ 

l=use  sliding  mode,   0  =  control  */ 

l=use  thnisters,  0=use  propellers  •/ 

l=use  thrusters  to  rotate  in  place  */ 

l=use  thrusters  for  lateral  motion  */ 

1=   go  to  WAYPOINT  without  WAITs  */ 

1=    go  to  WAYPOINT  */ 

l=hover  at  WAYPOINT  */ 


#i  f  defined (sgi ) 
extern  int    EMAIL 
♦  else 
extern  int    EMAIL 


/*  l=send  e-mail,  O=don't  send  e-mail  */ 
/*  can't  send  e-mail  via  OS-9  directly  */ 
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tendif 

excern  int    NOT_yET_REIMPLEMENTED   ;  /•   code  in  block  needs  reverif ication  •/ 

extern  double  TIMESTEP 


extern  int 


KEYBOARDINPUT 


;  /*   time  of  a  single  closed  loop       •/ 
/'   add  code  to  warn  if  exceeded  <«<   */ 

;  /*   l=read  keyboard  vice  mission  file   */ 


/*  file?  and  paths  */ 

extern  FILE  •  auvscript f i le; 

extern  FILE  *  auvordersf ile; 

extern  FILE  *  auvdatafile; 

extern  FILE  *  auvtextf ile; 

extern  FILE  *  controlconstantsf ile; 

extern  FILE  *  emailaddressf ile; 


•  FILE  *  s«^rialre--tf lie; 
extern  ir.z        serialpath  ; 
•extern  jr.r    sonarpath   ; 


V 


/ '  Variable?  and  data  structures 


.  '  buffers  cf  full  strings  for  byte  transfer  to  tactical  level  &  disk  file 
/*  'buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error 


extern 
extern 


tirrie_t 
struct    tir. 


systein_tiiTie 
*systeiT>_tmp 


struct   MFI_PIA 

( 

unsigned  short  pra 
unsigned  short  era 
unsigned  short  prb 
un:?^aned  short  crb 

); 


/•   4  Channels  of  DAC  ADA-1  DAC 
extern  unsigned  char   •dacl_a   ; 

/«   8  Channels  of  DAC  DAC-2B 
extern  unsigned  char  •dac2b_a  ; 

/•  It   Channels  of  ADC  ADA-1 
extern  unsigned  char   •adcl_a   ; 

/•  16  Channels  of  ADC  ADC-2 
extern  unsigned  short  •adc2_a   ; 

extern  unsigned  short  "viaO    ; 


/'  port 


/*  control  register  B 


extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  double 
extern  double 
extern  double 
extern  double 


telemetry_records_saved    ; 

inission_leg_counter ; 

replication_count 

end_test 

wrap_count 

t 

dt 

rpm; 

speed_limit 


/'  partial  structure  template  for  the  MFI  (only  interested  in  PIA  for  now)  •/ 


/*  port    register  A  -  data  direction  A  "/ 
/'  control  register  A  */ 


register  B  -  data  direction  B  */ 


V 
*/ 
•/ 
*/ 


101 


exce:;-,   double 
extern   double 
extern   int 
extern   int 


extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
exter.'". 
ex  t  e  rn 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 
extern 


double 
double 
double 
double 
double 
double 
douDle 
double 
double 
double 
dc'ubl  e 
double 
double 
double 
double 
double 
double 
double 
double 
double 
double 
double 
double 
doubl e 
double 


ma  i  n_ino  t  o  r_de  1 1  a  1 
ma i n_mo t o r_de 1 t a2 
ma  i  n_mo  t  o  r_vo 1 1 1 
ma  i  n_mo  t  o  r_vo 1 1 2 

X 

y 

z 

phi 

theta 

psi 

x_dot 

y_dot 

z_dot 

phi_dot 

theta_dot 

psi_dot 

speed 

u 

V 

w 

p 
q 

r 

delta_planes 
del ta_rudder 
port_rprr~ 
stbd_rprr; 

vertica2_thruster_vol ts 
lateral  thruster  volts 


/•  degrees 

•/ 

/*  degrees 

•/ 

/*  degrees 

*/ 

/*  degrees/sec  •/ 
/*  degrees/sec  */ 
/*  degrees/sec  */ 


/*  degrees/sec  */ 

/•  degrees /sec  */ 

/*  degrees/sec  */ 

/*  degrees  "/ 

/*  degrees  •/ 


)c_psi 

y. 

_x 

k. 

_v 

k. 

_z 

k 

_w 

k 

Itheta 

k 

.q 

extern  unsigned  snort   psi_bit_old 

ex:.^rn  douDie  dg_offset 

extern  double 
extern  double 
extern  double 

extern  double 
extern  douDie 
extern  double 
extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  double 
extern  double 

extern  double 

extern  double 

extern  double 

extern  double 

extern  int 


k_thruster_psi 

k_thruster_r 

k_thruster_rotate 

k_thruster_lateral 

k_thruster_z 

k_thruster_w 

k_p rope ller_h over 

k_surge_hover 

k_thruster_hover 
k_sway_hover 

k_sigma_r 
k_sigma_psi 
eta_steering 
Sigma 

mission_legs_total 


/•        values  initialized  in  parse_mission_script_commands  ()  */ 
extern  double  time_next_command       ; 


102 


exterr. 
extern 
extern 
extern 
extern 
extern 
e>:teri~, 
extern 
extern 
extern 
extern 


do-L^bie 
double 
double 
double 
double 
double 
double 
double 
double 
double 
double 


psi_comiTiand 

/• 

degrees 

•/ 

psi_command_hover 

/* 

degrees 

•/ 

x_cominand 

/• 

feet 

•/ 

y_coiniTiand 

/* 

feet 

•/ 

z_corrmand 

/* 

feet 

•/ 

s t bd_r pm_c omma nd 

port_rpm_comiriand 

pi  ane£_cominand 

/* 

degrees 

*/ 

rudder_cominand 

/• 

degrees 

•/ 

r  o  t  a  t  e_c  omir.a  nd 

/» 

degrees/sec 

•/ 

lateral_comir,and 

/• 

ft/sec 

•/ 

extern  double 

extern  double 

extern  double 

extern  double 


bow_lateral_thruster_cominand 

stern_lateral_thruster_coininand 

bow_vertical_thruster_comniand 

stern  vertical  thruster  command 


/•  volts  -24 . .24  */ 

/•  volts  -24 . .24  */ 

/*  volts  -24. .24  •/ 

/•  volts  -24 . .24  •/ 


extern  double 
extern  double 

extern  double 
exter:".  double 
T  X 1  e  1  -■ .  o  o  u  ;_■  _  e 
extern   double 


extern  ir.z 


extern 

in: 

extern 

int 

extern 

int 

extern 

int 

extern 

i  I'i  t 

extern 

mt 

extern 

int 

extern 

int 

extern 

int 

extern 

int 

extern 

double 

extern 

double 

extern 

double 

extern 

double 

extern 

dout^le 

extern 

int 

extern 

int 

extern 

int 

extern 

int 

extern  int 

exterri  int 
extern  int 
extern  int 
extern  int 
extern  int 


waypoint_di stance 
waypoint_angle 

track_angle 
a long_track_di stance 
cross_track_di stance 
standcf  f_di stance 

depth._error  ; 

roll_rate_0 

pitrn_rate_C' 

yaw_rat€_C' 

roll_0 

pitch_C 

2_val0 

swl 

error 

range 

bad_rng 

bad_updates 

range_index 

range: 

range2 

errorl 

errcr2 

avg_rng 

k_range 

range_array  [3000], • 


pointer 
speed_array 

PortAFlag 

tick 

curr_tick 

tickl 

tick2 

i 
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extern  int 
extern  long 
extern  long 
extern  double 
extern  short 
extern  char 
extern  int 

extern  int 


mask 

davedate 

davetime 

value 

day 

answer 

start_dwell 

socket_descriptor 
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ex'er.'T  int  sockec_accepted 

exLerr;  int  socket_streain 

ex'-^iT.  inr  socket_lengch 

extern  int  bytes_received 

extern  inc  bytes_read 

extern  int  bytes_written 

extern  int  bytes_left 

extern  int  bytes_3ent 

char  buffer_array  [FILEBUFFERSIZE] 

extern  char  buffer 

extern  int  buffer_size 

extern  int  buffer_index 

extern  int  variable£_parsed 

extern  char 


;/*  max  allowed  packet  size  */ 


buf f er_received 
virtual_world_remote_host_najne    (60  ] 
cominand  buffer  [300] 


;256];  --  not  implemented 
[3001; 


[300]  , 


extern  FILE         *  netstat_f iieptr ; 
extern  struct  sockaddr_in  server_address; 
extern  struct  hostent  *server_enti ty ; 


extern  char 
e  >i  t  e  r  ;■;  c  n  a  r 

extern  rnt 
extern  int 

extern  char 

extern  mt 

extern  double 


keyword       [81 ] ; 
emaii_address  [81]; 

shutdown_signal_received 
virtual_world_socket_opened  ; 

ptr_index; 

print_help; 

3peed_per_rpm;  /•  steady  state:   2.0  feet/sec  per  700  rpm 


extern  clock_t   previousloopclock; 
extern  clocK_t    current ioopclock; 

/*  ^-    24V  <=>  +-2  lb,  +  Volts  moves  thruster  in  +  direction,  all  identical  */ 


extern  double  AUV_bow_vertical 
extern  dcutie  AU"y_stern_vertical 
extern  double  AU\'_bow_lateral 
extern  double  A'JV  stern  lateral 


/*  chruscer  rpm 

/»  thruster  rpm 

/*  thruster  rpm 

/*  thruster  rpm 


/'  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal  •/ 


extern  douole  AlJV_ST1000_bearing 
extern  double  AliV_ST1000_range 
extern  double  AUV_ST1000_strength 

extern  double  AUV_ST72  5_bearing 
extern  double  AUV_ST72  5_range 
extern  double  AUV_ST725_strength 


/•  ST_1000  conical  pencil  bearing  •/ 

/•  ST_1000  conical  pencil  range   •/ 

/*  ST_1000  conical  pencil  strength*/ 

/*  ST_725  1  X  24  sector  bearing   */ 

/*  ST_725  1  X  24  sector  range    */ 

/*  ST_725  1  X  24  sector  strength  */ 


extern  mt 
extern  int 


audible_cominand 
auvscriptf ilequit 


■***************i 


r  »«*«**  1 


'•***••*•«****•******••*• 


*********»•••**•••»***/ 


iendif 
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G.  Makefile,0S9  for  Execution  Level  under  OS-9  Operating  System 

«  A'JV  execution  level  Makefile.  0S9 
«  Don  Brutzman  21    OCT  94 

«t  modified  OS-9  makefile  (originally  from  /hO/INET/SOURCE/) 

#  this  Makefile  is  necessary  to  resolve  network  libraries  compilation 

«  Note  that  OS-9  Make  syntax  is  nonstandard. 

DD      =  /hO 

Lib  =    -1=S (DD; /iib/sys. 1 

SCCKLIB  =    . . ./lib/socklib.l 

NE'T'LIE     =    .  .  .  /lib'netdb.l 

*?.::?        =    RELS 

P. ""' "  P,  = 

OLIR  =    S (DD) /cmds 

RFLAGS 

CFLAGS  =    -bp   -g 

£'■'.'?"£     =    Lvcv_3ti  xnit_sti  os9sender  os9server 

R£T:''J?.rE   -    recv_3tr.r         xrr,:t_str.r  os9sender.r        os9server.r 

iTake.date:    S'SOUFTE' 

T.  Ou  Z i .     TTi 3i  ^'.  r-  .  d 5  w €: 

alctals:  r'SOCKLI?   SiNETLIE^  glcbals.c  globals.h  defines. h  Makefile 

cc    S{CFLAG£;  globals.c  -r  -1= . . /S (NETLIB)  -  1= . . /$ (SOCKLIB)  $(LIB) 

^a:i:t_fur.ct:cns:  SiSOCKLIB,  $(NETLIE)  parse_f  unctions  .  c  globals.h  defines. h 
Makefile 

cc  S'CFLAGE:  parse_f unctions . c  -r  -1= ../$ (NETLIB)  -1= .. /S (SOCKLIB)  $(LIB) 

execution:  SiSOCKLIB]  $(NETLIB)   globals.r  parse_functions . r    execution. c 
globals.h  defines. h  Makefile 

cc  S(CFLAG£)  execution. c  alobals.r  parse_f unctions . r  -f=execution   \ 
-1= .  ./S (NETLIB;  -  1= .. /S (SOCKLIB )  $(LIB)  -Im 

«  clean; 

«       ra  ' . r  /hC /CMDS /execution 

«  clj  versions 

«  -K  =  ',  r,Bi(r-j    (default) 

*  -k=2  ee:2  0 

M    -t=/rO     puts  temporary  files  in  ramdisk  and  speeds  compilation 

a    f.r  saves  relocatable  modules  in  RDIR 

«  'SOURCE  :  S (SCCKLIB)  $ (NETLIB; 

*>       cc    2(CFLAG£i  S'.r  -f=2*  - 1=  ../$  (NETLIB )  -1=  .. /S  (SOCKLIB)  $(LIB) 

«       cc    S(CFLAGS)  execution. c  -r    -1= ../$ (NETLIB)  -1= ../$ (SOCKLIB)  $(LIB) 
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H.  Makefile.SGI  for  Execution  Level  under  SGI  Irix  5^   Operating  System 

«  A'.V  execution  ievel  Makefile,  sg: 

*  Don  Erutzman   27  OCT  94 

execution:  globals.c  parse_functions . c  execution. c  globals.h  defines. h  Makefile 
cc      -g  -lin  -cckr  -w  \ 

gloDais.c  parse_f unctions .0  execution. c  -o  execution 

«  The  'warnings'  make  option  gives  voluminous  diagnostics  which  are  useful 

#  in  preventing  mysterious  bugs  when  the  execution  code  is  ported  to  OS-9. 

«  Simj lar  to  lint . 

warnings : 

cc  -g    -Im  -cckr  -fullwarn  -wlint.p  \ 

globals.c  parse_f unctions . c  execution. c  -o  execution 


touch  globals.c  parse_functicns . c  execution. c 
make  execution 


:.ean : 

rrr.  execution  *.o 
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I. 


startup  OS-9  Reboot  System  Configuration  File 


-t  -np 


or-9  startup  file 

Lasi  modification  22  JUN  94 


Walt  Landaker  &  Don  Brutzman 


secime  -s 
link  shell  cio 


start  s^'stem  clock 

make  "shell"  and  "cio"  stay  in  memory 


*  the  following  are  already  in  RAM  memory  from  hard  disk  bootfile  /hO/OS9boot 


•  in:z  rO  hC  dC  tl  pi 
load  jr  i 1 s 
load  boot  GDI s / dd . rO 
m:  r  .  ramdisk  >/nil  >vnil4< 
tsmon  /tl  & 


*  initialize  devices 

•  make  some  utilities  stay  in  memory 
»  get  the  default  device  descriptor 

*  initialize  it  if  its  the  ram  disk 

•  start  other  terminals 


setenv  T£PJ<  vtlOO 
start  lar. 

wa:  t 

f  ■  ^^ 

1:.=  !:  ?y^  'mote 

srielj-   •■■'/teriT    -e= /h"- /sy?/erriTi?c  .  short 

I  oa :  V, 
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J.  .login  OS-9  User  Login  and  Unix  Alias  File 


*  Buvsin.i  .login  file 

«  for  OS-9  with  'sh'  shell 

*  Don  Brutzman   22  June  94 
h  1  p  t  o  r>'  4  C 


alias 

cat 

list 

a  1  1  b:^ 

cd 

chd 

alias- 

chiDod 

attr  -? 

alias 

clear 

cIs 

alias 

cp 

copy 

alias 

h 

history 

alias 

1 

dir  -ade 

a'^  .  i:S 

1  s 

dir  -ad 

alias 

m 

list 

alia? 

man 

help 

alias 

mkdir 

makdir 

alias 

more 

list 

alias 

lo 

logout 

alias 

nopause 

tmode  nopau 

alias 

pause 

tmode  pause 

alias 

ps 

procs 

alias 

pwd 

pd 

alias 

quit 

logout 

alias 

reboot 

shell  break 

alias 

ren 

rename 

a^  -  as 

rn 

del 

alias 

rmdir 

deldi  r 

alias 

o 

sh 

alias 

source 

sh 

alias 

type 

list 

*  following  doesn't  seerr;  to  work 
«    automatically  on  telnet  logins 
tmode  no;:  =  ose 

e  r  r. .'. 

ecno  "telnet  users  type:   nopause" 
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K.  auv_plot.gnu:   Execution  Level  Telemetry  Plotting  using  gnuplot 


iil-^namt:  auv_plot.gnu 

function:   GNUPLOT  V3.S  script  to  plot  AUV  telemetry  data 
to  screen  &  to  PostScript  files 

updated:   18  OCT  94 

author:   Don  Brutzman 

execution:   gnuplot>  load  •auv_plot .gnu* 
gnuplot>  reread 

Unix--  gnuplot  auv_ploc.gnu 

i-r-ri  -'T  ing: 

'xpsview   -wp  -skipc  -or  landscape  -/execution/AUV_telemetry .ps  & 
gh'jstview  -landscape  -/execution/AUV'_telemetry  .ps  & 

r  progran-  call:   system  l 'gnuplot  auv_plot  .gnu' )  ,- 

telernetry:   rr.ission. output .  telemetry  (AUV  telemetry  0.1  sec  interval) 
alternate:   m.ission .output . l_second   lAUV  telemetry  1.0  sec  interval) 


a.ternat-r  p^c^t  : 

output  files: 

related  files: 

lutpu:    arrhlv-r: 
gnui:.i'-.t    FA;); 


unix>   gnuplot      auv_plot_l_second.gnu 

AiJ\'_teienietry  .ps    &    '.eps   plots 

execution . c 

underwater   virtual   world 

ftp  :  //taurus.cs.np'S  .naw.mil/pub/auv/AlJV_telemetry.ps  .2 

ftp:  //f  tr.-.dartrtiouth  .edu  'pub,  gnuplot/f  aq/gpt_faq.html 


distriL'Ut  ion  :  ht  tp  :  //www.  cs  .dartnicuth  .  edu /gnuplot/ 

s»>aii«)>*4i«««>iiii««ti*«««titi»il»lili«»i«»«««««tit>««»«tt«t««tl«««««««««««tl«tl««««««tl»ll«« 

Plot  filenames: 

cmbined  AU"v'_telem!'rtry  .ps 

figure  1  AUV_x_y.eps 

figur-i   2  AUv'_t_x.eps 

figure  3  ALV_t_y.eps 

figure  4  AUV_t_z.eps 

figure  5  AUV_t_phi.eps 

figure   6  Ab''v_t_theta  .eps 

figure  7  ALA,'_t_theta_all  .eps 

figure  8  AU"\.'_t_ps:  .eps 

figure  ^   ALA/_t_lateral_thrusters .eps 

figure  10  AU\'_t_vertical_thrusters  .eps 

figuie  11  AlT\'_r_u  .eps 

figure  12  AUV_t_v.eps 

figure  13  AUV_t_w.eps 

figure  14  AU"v'_t_p  .eps 

figure  IS  AUv_t_q.eps 

figure  16  AUV_t_r.eps 

figure  11   AU\.'_t_delta_rudder  .eps 

figure  18  AUV_t_delta_planes.eps 

figure  19  AUV_t_rpm.eps 

figure  20  AUV_t_thrusters.eps 

itt«t«««i««««««««««««t««««««li««««««««««t««»«t««t«««ltf««««f«««««««t««««««i« 

setup: 

set   terminal   xli 

Sr-r    time 
set    grid 
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set  data  style  Imespoints 

sec  samples  10        «  data  point  plotting  frequency 

set  xlabel  'East  ->  (y_worid)  |ft)' 

s*-t  ylabel  "North  '  (x_world)  [ftl' 


set  title   "NPS  AU\'  telemetry   1'  26,. 8 

i:!''   "mission -O'jtp'jt .  telemetry  ■       using  4:3 

faus-r  -1  "hit  enter  to  continue  with  plot  2  " 

sv.   xlabel  "time  t  (seconds'" 
set  ylabel 


St-r  title   "NPS  AUV  telemetry   2"  26,. 8 

plor   "miESjLun  .output .  telen..rtry"       using  2:3   title 
"mission  .output .  telenietry"       using  2:15  title 
pc^se  -1  "hit  enter  to  continue  wicn  plot  J  " 

se:  t::le   "NP.^  AUV  telemetry   3"  26,. 8 

!:■.':.*   "m,s2i':.r.. output  .telemetry"       using  2:4   title 
"mission. output .telemetry"       using  2:16  title 
Tr.ws^   -1  "hit  enter  to  continue  with  plot  4  " 


title  "x  vs  y  (geographic  position  plot) 


26,  .8 


set  title   "NPS  AUV  telemetry   4 
i-  '.:'.      "m.ssion  .  outP'Ut .  telemetry" 

"rriss ion. output  .telemetry" 

"mission .output .telemetry" 

Tassicn. output .orders" 
pause  -1  "hit  enter  to  continue  with  plot  5 


2:5   title 
2 :17  title 


using 

using 

using  2:22  title 

using  1:3   title 


set  titlt   "NPS  AUv'  telemetry   5"  26,. 8 

p^ot   "m.ission  .output .  telem^etry"       using  2:6   title 
"iT'iss-on  .  output  .  telemetry"       using  2:16  title 
f.ajse  -1  "hit  enter  to  continue  with  plot  6  " 


s^t  title   "NP?  A',.'\'  teleiTietry   6"  26,. 8 
T- -  - 1   ":T,iss:cr. .  output .  telen.etry"       using 
•iT..ssicn.cutput . telem.etry"       using 
pause  -i  "hit  enter  to  continue  with  plot  7  ■ 

set  title   "NPS  AU\'  telemetry  7"  26,. 8 
plc<t   "mission.output .  telemetry" 

"rr.ission  .  output  .  telemetry" 

"mission .output . telemetry" 

"mission  .  ;>utF>ut .  telem.etry" 

"rr issuer..  :,;tput  .telemetry" 

"m^ss.cr. .cutp'Ut  .orders" 
stei:  s 
pa„se  -1  "hit  enter  tc   continue  with  plot 


■:7   title 
,:19  title 


't  vs  X     [ft] 

' t  vs  X  dot  I  ft/sec] 


't  vs  y     [ft]     • .    \ 
't  vs  y_dot  [ft/sec]" 


't  vs  z      (depth) 
't  vs  z_dot  (depth  rate) 
't  vs  delta_planes 
't  vs  ordered  depth 


't  vs  phi  (roll:   X  ajcis) 

't  vs  phi_dot    (roll    rate) 


using 

2 

7 

title 

"t 

using 

z 

19 

title 

"t 

using 

2 

5 

title 

"t 

using 

2 

11 

title 

"t 

using 

0 

22 

title 

"t 

using 

1 

3 

title 

"t 

[ft] 

• 

\ 

[ft/sec 

• 

\ 

[deg] 

" 

\ 

[ft] 

• 

with 

steps 

[deg]     ",  \ 
[deg/sec] " 


't  vs  theta     (elevation  angle)  [deg]     ',  \ 
't  vs  theta_dot  (elevation  ratei   [deg/sec]' 


vs  theta      (elevation  angle)  [deg] 

vs  theta_dot  (elevation  ratej   [deg/sec] 

vs  z         (depth)  [ft] 

vs  w         (heave)  [ft/sec] 

vs  delta_planes  [deg] 

vs  ordered  depth  [ft] 


\ 
\ 
\ 
\ 
\ 
with 


s-rt  title  "NPS  AUv?  telemetry  8' 
p.i;t  "mission  .output .  telem.etry" 
•mission.output .telemetry" 
"m.iss ion. output .  telemetry" 
•mission .output . telemetry" 
"mission.output .telemetry" 
•mission.output .orders' 


26,  .8 


using  2:8  title  "t 

using  2:20  title  "t 

using  2 : 14  title  "t 

using  2:10  title  "t 

using  2:21  title  "t 


vs  psi 


(azimuth:  z  axis) 


vs  psi_dot  (azimuth  rate) 


vs  r 
vs  V 
vs  delta  rudder 


(yaw  rate) 
( sway ; 


pause  -1  'hit  enter  to  continue  with  plot  9 


using  1:2  title  "t  vs  ordered  heading 


set  title   • 

plot 

•missi 

•missi 

•missi 

•missi 

•missi 

steps 

\ 

•missi 

steps 

\ 

•missi 

pause 

-1  •hi 

NPS  AUV  telemetry   9' 

on. output .telemetry" 
on. out put .telemetry" 
on. out put .telemetry" 
on. output .telemetry" 
on. output .telemetry" 

on. output -telemetry" 


26,  .8 


using  2:8  title  "t  vs  psi 

using  2:20  title  •t  vs  psi_dot 

using  2:14  title  't  vs  r 
using  2:10  title 


't  vs  V 
using  2:27  title  't  vs 


(azimuth:  z  axis) 
(azimuth  rate) 
(yaw  rate) 
( sway ) 
bow  lateral  thruster 


using  2:28  title  'c  vs  stern  lateral  thruster 


on. output .order s^         using  1:2  title  't  vs  ordered  heading 
t  enter  to  continue  with  plot  10  • 


set  title   •NPS  AUV  telemetry  10^  26,. 8 

plot   'mission.output .telemetry       using  2:5 


[deg] 

',  \ 

[deg/sec] 

',  \ 

[deg/sec] 

■,  \ 

[  ft/sec] 

■,  \ 

[deg] 

',  \ 

[deg] 

'  with  steps 

title  't  vs  z  (depth) 


[deg] 
I deg /sec 
[deg/sec 
[  ft/sec] 
[volts] 

•,  \ 
',  \ 
•,  \ 
',  \ 
•  with 

(volts] 

•  with 

[deg] 

•  with  steps 

[ft] 


no 


•m:.££iori  .output .  telemetry  •       using  2:11  title  "t  vs  w  (heave)  (ft/sec)',  \ 

•rr.i  SEion .  output  .  telemetry*       using  2:25  title  't  vs   bow  vertical  thruster  (volts)  •  with  steps,  \ 

•mission. output . telemetry'       using  2:26  title  "t  vs  stern  vertical  thruster  Ivoltsj  ■  with  steps,  \ 

"mission .output .orders'         using  1:3  title  't  vs  ordered  depth  (ft)    •  with  steps 

pause  -1  'hit  enter  zo   continue  with  plot  11  ' 

£►-'  ritiT  'Kys   AUV  telemetry  11'  2t,.6 

]-.l.:.:   'mission  .output .  telemetry '       using  2:9   title  't  vs  u  (surge)  [ft/sec]' 

ji'ause  -1  'hit  enter  to  continue  with  plot  12  ' 

set  title   'NPS  AUV  telemetry  12'  26,. 8 

p'.'-'t      'mission  .output .  telemetry '       using  2:10  title  't  vs  v  (sway)  (ft/sec)* 

p.ause  -1  'hit  enter  to  continue  with  plot  13  • 

set  title  'NPS  AUV  telemetry  13*  26,. 8 

plot   'mission. output .telemetry'       using  2:11  title  't  vs  w  (heave)  [ft/sec]* 

pause  -1  'hit  enter  to  continue  with  plot  14  ' 

set  title   'NPS  AUV  telemetry  14'  26,. 8 

plot   'mission. output . telemetry'       using  2:12  title  't  vs  p  (roll  rate)  [deg/sec]' 

pause  -1  'hit  enter  to  continue  with  plot  15  ' 

set  title   'NPS  AUV  telemetry  15'  26, .8 

vl"      'rriE^irr:  .C'Utput  .telemetry       using  2:13  title  't  vs  q  (pitch  rate)  (deg/sec)" 

t  .-i:.L-.-  -I  "hit  T-n'er  tc-  continue  with  plot  It  ' 

St'  t-tiT   'NPS  AU"v  telemetry  16'  2fc,.8 

T  .'•'      'mis^i^r^n  .output  .telemetry       using  2:14  title  't  vs  r  (yaw  rate)  (deg/sec)" 

pajs-  -'^    •n..t  enter  to  continue  with  plot  17  " 

se:  t.tlt  't-irS   k'SJ   teienietr>'  l''*  26,  .9 

].j.:~      "r.^ss:  :r.. output -telemetry "       using  2:21  title  "t  vs  delta_rudder  bow  (deg)" 

;:r._3T  -:  'hit  enter  f-  continue  with  plot  IE  ' 

set  tit^e   'N?5"  AL^V  telemetry  18'  26,. 8 

plot   'mission .output . telemetry '       using  2:22  title  't  vs  delta_planes  bow  (deg)' 

pause  -1  'hit  enter  to  co.ntinue  with  plot  19  ' 

s«r:  titlt  'K't-3    AUV  telemetry  19'  26,. 8 

{.i-i't  'niissicr.  .c  utput .  telemetry '  using  2:23  title  't  vs  rpm_left  (iiDm)",  \ 
•mission. C'Utput .telemetry"  using  2:24  title  't  vs  rpm_right  (rpm)',  \ 
•mission  .output  .orders'         using  1:4   title  •t  vs  rprT^_ordered  (rpm)"  with  steps 

y.-^i-^'i    -1    "hit  en:tr  to  continue  with  plot  20  ' 

ser  title   'NPS  AUV  telemetry  20^  26,. 8 

!:>•:   •mission.'.iutpiut .  teleniet  ry^  using  2:25  title  •t  vs   bow  vertical  thruster  (volts)'  with  steps,  \ 

•mission. output .telemetry  using  2:26  title  •t  vs  stern  vertical  thruster  (volts) •  with  steps,  \ 

•n.ission  .output  .teleiTietry  using  2:2''  title  't  vs   bow  lateral  thruster  [volts]'  with  steps,  \ 

•mission .output .telemetry  using  2:28  title  't  vs  stern  lateral  thruster  [volts]"  with  steps 

pause  -1  "hit  enter  to  plot  postscript  files  &  quit" 

»««t««ii««««««t««««t««««t«*«««t«i««««t«««««t««««««««««««««*««««««««««««t«««««« 
«««««t«t*«««««ii««««t««««««««««««««««i«««««««««««««««*««««««i««««««*«««««««««« 

pause  0  'plotting  comcined  plot  AUV_telemetry.ps  ...' 

Clear 

«  « 

«  uncomment  to  choose  one  of  the  following  two  options: 

«  « 

«   601  size  for  xpsview 

•  set  size  0.6,  0.6 

•  set  terminal  postscript  portrait   color  "Courier"  14 
»  set  title   "NPS  AUV  telemetry"  29,. 8 

«  « 

•  full  page  for  printing 

set  terminal  postscript  landscape  color  "Courier"  20 
set  title   "NPS  AUV  telemetry"  16,  .8 
• « 

set  output  "AUV_telemetry.ps" 

piause  0  "page  1* 

set  xlabel   "East  ->  (y_world)  (ft)" 


111 


s^r  Viatel  'North  ---  (x_world)  [ft]' 
Set  title  'UPS    klTJ    telemetry   1'  14, 
plot   'mission . output . telemetry* 


set  xiabel  "time 
set  ylabei 


( seconds ' 


using  4:3   title  "x  vs  y  (geographic  position  plot)' 


^■ause    0    'page    2" 

set    title      'NFS   AUV   telemetry      2' 
piot      'mission. output  .  telemietry' 
•miss  ion. output .telemetry' 


14,  .8 


using  2:3   title  't  vs  x     [ft]     ',  \ 
using  2:15  title  't  vs  x_dot  [ft/sec]' 


F-aus-^  0  'page  ?' 

set  title   'NPS  AUV  telemetry   3"  14, 
plot   'mission. output . telemetry' 
•miss  ion. output . telemetry' 


using  2:4   title  't  vs  y 
using  2:16  title  't  vs  y_dot 


ft]     •,  \ 
ft/sec]  • 


s-r  title   'NF.-  Airv  telemetry   4'  14,. 8 
;  .  .r   'mi  ££i  :;r. .  ."Utpjt  .  t<rler-:  ;  y  • 
■mission . output . telemetry' 
•missiLin  .output  .telemetry 
•n.ission .  :.utpuc  .orders" 

f.ause  0  'page  5' 

set  title   'NPS  Ab'V  telemetry   5'  14, 
plot   'mission .output . telemetry' 
'n'-issior. .  output .  telemetry' 


using  2:5   title  't  vs  z      (depth)  [ft]     ',  \ 

using  2:17  title  't  vs  2_dot  (depth  rate)  [ft/sec]',  \ 

using  2:22  title  't  vs  delta_planes  Ideg]    ',  \ 

using  1:3   title  't  vs  ordered  depth  [ft]     *  with  steps 


using  2:6  title  't  vs  phi     (roll:  x  axis) 
using  2:18  title  't  vs  phi_dot  (roll  rate) 


Ideg]     •,  \ 
[deg/sec] ' 


i:a':se  0  'tiage  6' 

set  titl'-   'Nf-S  AL"v'  telenietry   6'  14, 
plot   'mission . output . telemetry' 
•rr.iss ion. output  .telemetry' 


using  2:7   title  't  vs  theta      (elevation  angle)  (deg] 
using  2:19  title  't  vs  theta_dot  (elevation  rate)   [deg/sec]' 


pause  C  'P'age  7* 

set  title   'NFS  A'J'v'  telemetry   7' 
f  lot   'mission  .  output .  telenietry' 
•mission . output . telemetry" 
'mil SE ion  .output .  teleitiet  ry ' 


'miss 
'n.iss 


stej:  s 


or.  .output  .telemetry' 
utt  _t . telemetry" 


'iTiission  .output  .orders' 


14,  .8 


using  2:7   title  't  vs  theta      (elevation  angle)  [deg] 
using  2:19  title  't  vs  theta_dot  (elevation  rate)   [deg/sec] 


using  2:5  title  't  vs  z         (depth) 

using  2:11  title  't  vs  w         (heave) 

using  2:22  title  't  vs  delta_planes 

using  1:3  title  't  vs  ordered  depth 


1ft: 
[ft/sec] 
[deg] 
[ft] 


\ 
\ 
\ 
\ 

\ 
with 


pause  0  'page  &" 

set  title  'NPS  AUV  telemetry  8"  14, 
PaOt  'mission .output .telemetry' 
•mission  .outpiut .  telem.etry' 
•mission. out put .telemetry' 
•mission. output .telemetry' 
•m, ssion . cut put . t el  erne t  ry ' 
•mission . output .orders" 


using  2:8  title 
using  2 :20  title 
using  2:14  title 
using  2:10  title 
using  2:21  title 
using  1:2   title 


t 

vs  psi     (azimuth:  z 

axis) 

Ideg] 

,  \ 

t 

vs  psi_dot  (azimuth  rate) 

[deg/sec] ' 

,  \ 

t 

vs  r       (yaw  rate) 

[deg/sec] ' 

,  \ 

t 

vs  V       (  sway ) 

I  ft/sec]' 

,  \ 

t 

vs  delta_rudder 

Ideg] 

,  \ 

t 

vs  ordered  heading 

Ideg] 

with  steps 

pfluiT 

0  • 

ser  t 

.tie 

p.  It 

•m,:. 

•mi 

•mi 

"rill 

•mi 

steps 

\ 

•mi 

Steps 

\ 

•mi 

pag-r  9" 

'NPS  AUV  telemetry  9'  14, 
ssicn .output . telemetry' 
ssion .output . telemetry' 
ss ion .output . telemetry' 
ss 1  on . output . telemetry" 
ssion. output .telemetry' 

ssion. output .telemetry* 

ssion. output .orders' 


using 

2 

6 

title 

■t 

vs 

psi     (azimuth:  z  axis) 

Ideg]     *,  \ 

using 

2 

■20 

title 

't 

vs 

psi_dot  (azimuth  rate) 

[deg/sec]*,  \ 

using 

2 

.14 

title 

't 

vs 

r       (yaw  rate) 

[deg/sec]*,  \ 

using 

2 

■10 

title 

't 

vs 

V       ( sway ) 

[  ft/sec]*,  \ 

using 

2 

:27 

title 

*t 

vs 

bow  lateral  thruster 

[volts]   •  with 

using  2:28  title  't  vs  stern  lateral  thruster 
using  1:2   title  *t  vs  ordered  heading 


pause  0  •page  10' 

set  title  'NPS  AUV  telemetry  10'  14,. 8 
plot  'mission. output . telemetry ' 
•mission .output . telemetry' 
•mi ssion. output .telemetry' 
•miss ion. output .telemetry* 
•mission. output .orders* 

pause  0  *page  11' 

set  title   'NPS  AUV  telemetry  11*  14, 

riot   'mission. output .telemetry' 


using  2:5  title  "t  vs  z  (depth) 

using  2:11  title  't  vs  w  (heave) 

using  2:25  title  't  vs   bow  vertical  thruster 

using  2:26  title  "t  vs  stern  vertical  thruster 

using  1:3  title  *t  vs  ordered  depth 


using  2:9  title  *t  vs  u  (surge)  [ft/sec]* 


[volts]   •  with 
[deg]    •  with  steps 


(ft]       •,  \- 

Ift/sec]*,  \ 

[volts]  *  with 

steps, 

\ 

[volts]  •  with 

steps, 

\ 

[ft]    •  with 

steps 
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pause  0  "page  12' 

S'?t  title   'NP?  AUV  teiemecry  12'  14,. 8 

f'L:-'      'mission  .output  .  teierrietry*  using   2:10    title    't   vs   v    (sway)     |ft/sec]* 

p.a^se  C  "page  1?  " 

set  title   'NPS  AUV  telemetry  13"  14,. 8 

plot   •mission .output . telemetry*       using  2:11  title  "t  vs  w  (heave)  [ft/sec]" 

pause  0  'page  14* 

set  title   'NFS  AUV  telemetry  14"  14,. 8 

r-lot   "mission .output .telemetry ■       using  2:12  title  't  vs  p  (roll  rate)  [deg/sec]' 

rause  0  "page  1?* 

set  title   'NPS  AUV  telemetry  15"  14,. 8 

piOL   -m-ssion .output .telemetry*       using  2:13  title  'l   vs  q  (pitch  rate)  [deg/sec]* 

pause  0  'page  16* 

set  title   *NPS  AUV  telemetry  16*  14,. 8 

plot   'mission. output .telemetry*       using  2:14  title  *t  vs  r  (yaw  rate)  [deg/sec]* 

pause  0  'page  1"" 

set  title   "NP?  A'JV  telemetry  l"?*  14,. 8 

plot   'mission. output .telemetry'       using  2:21  title  't  vs  delta_rudder  bow  [deg]* 


pause  C  'page  If" 

St-  -i:1t  'NPS    AU\'  telemetry  16'  14,.  8 

p  ajt   'rr  .ssion  .  output .  telemetry* 

•'.  -i..i-r    '    "i^aae    '.^' 

i-t    t-t-T      "NrJ"   A'JV   telemetry   19'    1 

r  .  :;r      'r^ission  .cutput  .  telemetry' 

'TTission  .tutput  .telem.etry* 

"mission . output .orders* 

pause  ?  *page  2{:* 

se:  title   *NF?  AU^v'  telemetry  20*  1 

plot   *m.ission  .output  .  telemetry* 

•m.ission  .  output .  telem.et  ry  • 

'm.issior. .  output .  telemet  ry ' 

'mission  .  :utput .  telemietry ' 


using  2:22  title  't  vs  delta_planes  bow  [deg]" 


using  2:23  title  't  vs  rpm_left  [rpm]',  \ 
using  2:24  title  "t  vs  rpmi.right  [rpm]',  \ 
using  1:4   title  't  vs  rpm_ordered  [rpm]'  with  steps 


using  2:25  title  't  vs   bow  vertical  thruster  [volts]'  with  steps,  \ 

using  2:26  title  't  vs  stern  vertical  thruster  [volts]*  with  steps,  \ 

using  2:27  title  *t  vs   bow  lateral  thruster  (volts;*  with  steps,  \ 

using  2:28  title  't  vs  stern   lateral  thruster  (volts)*  with  steps 


pause  C  'p.ottmg  individual  encapsulated  postscript  files... 

s-r".  rit.r  'li?3   KJ\'   teiemetry  29,. 8 

se'  terminal  postscript  eps  color  *Courier"  14 

fdjse  Z    'fig-ie   1  AU'^_x_y  .  eps  * 

set  output        "AUV_x_:>'.eps* 

set  xlabel   "East  ->  (y_world)  [ft]" 

set  ylabel  "North  ->  (x_world)  [ft]" 

set  title   "NPS  AUV  telemetry   1"  26,. 8 

plot   "mission. output -telemetry" 


using  4:3   title  "x  vs  y  (geographic  position  plot) 


set  xlabel  "time  t  (seconds)* 
set  ylabel 

pause  0  "figure  2  AUV_t_x.eps" 
set  output         •AUV_t_x.eps* 
set  title   "NPS  AUV  telemetry   2"  26,. 8 
plot   "mission  .output .  telemeti-y* 
"mission .output .telemetry" 

pause  0  "figure  3  AUV_t_y.eps" 
Sfet  output  •AUV_t_y.eps" 
set  title  "NPS  AUV  telemetry  3"  26,. I 
plot  "mission .output .telemetry" 
•mission .output . telemetry" 

pause  0  'figure  4  AUV_t_z.eps' 

set  output        •AUV_t_z.eps' 

set  title   "NPS  AUV  telemetry  4*  26,. I 

plot   "mission. output -telemetry" 


using  2:3   title  "t  vs  x     [ft] 
using  2:15  title  "t  vs  x_dot  [ft/sec] 


using  2:4  title  't  vs  y     [ft] 
using  2:16  title  "t  vs  y_dot  [ft/sec] 


using  2:5  title  "t  vs  z 


(depth) 


Ift] 
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•n.ission.  output  .telemetry* 
•mission .output . telemetry 
•mission. output .orders' 

pause  0  'figure  5  AUV_t_phi.eps* 
set  output        •AUV_t_phi.eps' 
s-i-r  t^tl-^   "NFS  AUV  telemetry   5*  26, 
plot   'Biission  .output .  telemetry  * 
•mission .output . telemetry* 


using  2:17  title 
using  2:22  title 
using  1:3   title 


using  2:6   title 
using  2:16  title 


't  vs  z_dot  (depth  rate) 
't  vs  delta_planes 
't  vs  ordered  depth 


[ft/sec]*,  \ 
[deg]  *,  \ 
[ft]     *  with  steps 


't  vs  phi     (roll:  X  axis)  [deg]    •, 
■t  vs  phi_dot  (roll  rate)     (deg/sec] • 


pause  0  'figure   6  AUV_t_theta.eps* 
£<=!  output         *AUV'_t_theta  .eps" 
s*-t  rirlc   'NFS  AU\'  telemetry  6*  26,. 8 
plr-r   'T.ission. output  .telemetry *       using  2:7   title 
•jrcssion  .output  .telemetry*       using  2:19  title 

pause  0  'figure  7  AUV_t_theta_all.eps* 
set  jutj-ut  *ALJV_t_theta_ali  .  eps* 
set  title  'NFS  AUV  telemetry  7*  26,. 8 
Ij.,  I   'mission  .output .  telemetry* 

•n^i ss i  ;>r. .  -utpur  .  t el emet ry  * 

•tt..  zx  :  ;r. .  r-utp.ut  .  t  eieni-rt  :y " 

"m:  ssior. .  outp^ut  .  telerriet  ry ' 

•rcission  .c-utput .  telemet  ry ' 

'TIS.:.:  .ir  .  -.utiJUt  .oio-ri  s' 


using  2 :7  title 

using  2:19  title 

using  2:5  title 

using  2:11  title 

using  2:22  title 

using  1:3  title 


■t  vs  theta     (elevation  angle)  [deg]    *,  \ 
■t  vs  theta_dot  (elevation  rate)   [deg/sec]* 


't  vs  theta     (elevation  angle) 
"t  vs  theta_dot  (elevation  rate) 
"t  vs  z         (depth) 
't  vs  w         (heave) 

■t  vs  delta_planes 
't  vs  ordered  depth 


[deg] 

,  \ 

[deg/sec] * 

,  \ 

[ft] 

,  \ 

[ft/sec]  * 

,  \ 

[deg] 

,  \ 

[ft] 

with 

pause  0  *figuie   6  AUV_t_psi.eps* 
set  cutpJt        •A'J\'_t_psi  .eps* 
set  title   'NFS  A'l?;  telemetry   8'  26, 
i;I:c  •jr.ission.outp'Ut  .telemetry" 
ssion.':  utji-ut  .telemetry' 
ssi or. .output  .telemetry' 
ssicr. .  output  .  telemet  ry ' 
•mission .output . telemetry* 
'mission .output .orders " 


using  2:8  title 

using  2:20  title 

using  2:14  title 

using  2:10  title 

using  2:21  title 

usina  1 : 2  title 


't  vs  psi 


(azimuth:  z  axis)  [deg] 


■t  vs  psi_dot  (azimuth  rate)  [deg/sec] 

■t  vs  r       (yaw  rate)  [deg/sec] 

"t  vs  V       (sway)  [  ft/sec] 

't  vB  delta_rudder  (degj 

't  vs  ordered  heading  [deg] 


\ 
\ 
\ 
\ 
\ 
with  steps 


pause 


se 

Pl 


0  "figure   9  AUv'_t_iateral_thrusters  .eps* 
utput        •AU\'_t_lateral_tnrusters  .eps" 
itie   'NFS  AU\'  telemetry   9'  26,. 8 
'mission . output . telemetry*       using 
•miss  ion. output .telemetry*       using 
'Rission . output . telemetry'       using 
'inissior. .  output .  telemetry'       using 


•iTi  ESi  on  .output .  telemetry* 
'm.ssion. output .  teleiTietry* 
'n.ission. output  .orders* 


2 

8 

title 

2 

20 

title 

2 

14 

title 

2 

10 

title 

using  2:27  title 
using  2:28  title 
using  1:2   title 


't  vs  psi     (azimuth:  z  axis)  [deg]     ",  \ 

"t  vs  psi_dot  (azimuth  rate)  [deg/sec]*,  \ 

"t  vs  r       (yaw  rate)  [deg/sec]",  \ 

't  vs  v       (sway)  |  ft/sec]",  \ 

■t  vs   bow  lateral  thruster  (volts]   "  with 


■t  vs  stern  lateral  thruster 
't  vs  ordered  heading 


[volts]   •  with 

[deg]     ■  with  steps 


paus-r  'j   'figure  IC  AUV_t_vertical_thrusters.eps' 

set  output        •AUV_t_vertical_thrusters.eps' 

set  title   'NFS  AL^V  telemetry  10"  26,. 8 

f.lc>r  'mission . output .  teleiTietry '  using  2:5  title 
'mssion  .output  .  telemetry '  using  2:11  title 
•itission. output  .telemetry '  using  2:25  title 
'mission. output .telemetry'  using  2:26  title 
•mission. output .orders'         using  1:3   title 

pause  0  'figure  11  ALA/_t_u  .eps' 

set  output        •AUV_t_u .eps' 

set  title  'NFS  AUV  telemetry  11"  26,-8 

plot  'mission. output .telemetry'       using  2:9  title 

pause  0  'figure  12  AUV_t_v.eps" 

set  output         'AUV_t_v.eps" 

set  title  'NFS  AUV  telemetry  12"  26,-8 

plot   'mission. output .telemetry"       using  2:10  title 

pause  0  'figure  13  AUV_t_w-eps" 

set  output        'AUV_t_w-eps' 

set  title   'NFS  AUV  telemetry  13'  26,-8 

plot  'mission. output -telemetry'       using  2:11  title 

pause  0  'figure  14  AUV_t_p.eps" 
set  output        •AUV_t_p.eps' 


't  vs  2  (depth)  [ft]     • 

't  vs  w  (heave)  [ft/sec]" 

't  vs   bow  vertical  thruster  [volts]  • 

't  vs  stern  vertical  thruster  [volts]  * 

't  vs  ordered  depth  [ft]     * 


't  vs  u  (surge)  [ft/sec] 


't  vs  v  (sway)  [ft/sec]' 


't  vs  w  (heave)  [ ft/sec)* 


\ 

\ 

with 

steps, 

\ 

with 

steps, 

\ 

with 

steps 
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set    title      'NFS  AUV  telemetry  14"  26,. 8 

P'.ot   "inissiori  .output .  telemetry  •       using  2:12  title  't  vs  p  (roll  rate)  [deg/sec]* 

pause  0  'figure  15  AUV_t_q.eps" 

y^'    -utpu'        •A'J\'_t_q.eps" 

.,-•  '.'..'r      •:<}.'   AUV  telemetry  15'  2fc,.8 

p-';t   •n.ission  .output .  telemetry  ■       using  2:13  title  "t  vs  q  (pitch  rate)  [deg/sec]* 

pause  0  'figure  Ifc  AUV_t_r.eps" 

ser  t.utp'Ut        •AUV_t_r  .epis" 

set  titl>r   'NFS  AUV  telemetry  lb"  26,. 8 

plot   'missior.  .c^utput  .telemetry'       using  2:14  title  't  vs  r  (yaw  rate)  [deg/sec]* 

r.ause  C  'figure  17  AUV_t_delta_rudder.eps" 

set  output        'AUV_t_delta_rudder.eps' 

se-  :^:.-   'NPT  AL'V  telemetry  1""  26,. 8 

plot   'mission .output .telemetry'       using  2:21  title  *t  vs  delta_rudder  bow  [deg] * 

pause  0  'figure  16  AUV_t_delta_planes.eps' 

set  output        'AUV_t_delta_planes.eps* 

set  title   'NFS  AUV  telemetry  18'  26,. 8 

plot      'mission .output .telemetry"       using  2:22  title  *t  vs  delta_planes  bow  [deg]* 

p.ause  C  'figure  19  AUV_t_rpm.eps' 

set  output        *AU'v'_t_rpm  .  eps' 

set  title   'NFS  AUV  telemetry  19"  26,. 8 

pict  'mission .output . telemetry '  using  2:23  title  "t  vs  rpm_left  [rpm]',  \ 
•mission .output . telemetry'  using  2:24  title  't  vs  rpm_right  [rpm]',  \ 
'n.iEE. or.  .output  .orders'         using  1:4   title  "t  vs  rpm_ordered  [rpm]'  with  steps 

P=u3t  i    'figuie  2C  AU'w'_t_thrusters .  eps' 

set  y^ti  ^t  'AUV_t_thrusters.eps' 

se-  Title   "NF"  AUV  telemetry  20'  26,. 8 

^..t  'miss:  :..^  .output .  telemetry'  using  2:25  title  "t  vs  bow  vertical  thruster  [volts]'  with  steps,  \ 
'mission .output .telemetry'  using  2:26  title  *t  vs  stern  vertical  thruster  [volts]'  with  steps,  \ 
'mission  .•:'Utput  .teiemetry'  using  2:27  title  "t  vs  bow  lateral  thruster  [volts]'  with  steps,  \ 
'mission .output .telemetry'       using  2:28  title  't  vs  stern   lateral  thruster  [volts]'  with  steps 

J.  :tJ3-  C  'a'nu;:^  ".  a  j\'_;: .ot  .gnu  conij;.ltrte.  ' 
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IV.   UNDERWATER  VIRTUAL  WORLD  GRAPHICS 

A.  Introduction 

The  Open  Inventor  2.0  applications  program  interface  (API)  is  used  for 
generating  3D  computer  graphics  images  representing  the  virtual  world 
(Wernecke  94a,  94b).   Open  Inventor  was  chosen  for  the  virtual  world  graphics  engine 
due  to  its  being  built  upon  the  Open  GL  graphics  language,  expected  portability  to 
multiple  hardware  architectures  and  operating  systems,  SGI-standard  scene  description 
language,  object-oriented  design,  broad  flexibility  and  strong  support.   The 
Open  Inventor  scene  description  language  (.iv)  is  likely  to  become  a  compatible 
standard  with  numerous  graphics  engines  and  the  nascent  Virtual  Reality  Modeling 
Language  {.vrml),  a  research  extension  to  Hypertext  Markup  Language  (.html). 

Embedded  in  the  graphics  viewer  code  is  a  Distributed  Interactive  Simulation 
(DIS)  2.0.3  multicast  network  API  which  permits  viewer  to  receive  robot  update 
information  nearly  anywhere  the  Internet  via  the  Multicast  Backbone  (MBone).   After 
the  configuration  file  .sd.tcl  is  installed,  the  viewer  program  can  be  launched 
automatically  via  the  MBone  session  directory  (sd)  advertisement  application,  enabling 
simple  remote  viewer  execution  and  connection  to  an  already-running  virtual  world. 
Future  work  includes  adding  a  WWW-compatible  scene  and  object  loader,  and  the 
capability  to  receive  URLs  for  retrieval,  either  via  DIS  updates  or  from  objects 
encountered  by  the  robot  that  are  already  in  the  scene  graph. 
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B.  viewer.C  Object-Oriented  Real-Time  3D  Computer  Graphics  Viewing 

Program  using  Open  Inventor  2.0 


1 1 !  1 .  1 1 1 1 !  i  1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 11 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  n  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 

I  * 

PrograiTi:  viewer.C 

Description:     Open  Inventor  viewer  for 

NPS  AUV  Underwater  Virtual  World 

Ajtnor:  Don  Brutzman 

Pev:?ed:  26  October  94 

£y  3 :  eni :  I  r  i  x  5  .  2 

Compiler:  AMSl  C+*  /  Open  Inventor  API 

Compliation;  irix>  make  viewer 

References:  (1)  IEEE  Protocols  for  Distributed  Interactive  Simulation  (DIS) 
Applications  version  2.0,  Institute  for  Simulation  and 
Training,  Universit  of  Central  Florida,  Orlando  Florida, 
28  May  1993. 

(2)  Macedonia,  Michael,  Zeswitz,  Steven,  and  Locke,  John, 
Distributed  Interactive  Simulation  (DIS)  multicast 
version  2.0.3,  Naval  Postgraduate  School,  February  94. 

(3)  Zeswitz,  Steven,  "NPSNET:  Integration  of  Distributed 
Interactive  Simulation  (DIS)  Protocol  for  Communication 
Architecture  and  Information  Interchange.'  master's  thesis. 
Naval  Postgraduate  School,  Monterey  California,  28  May  1993. 

(4)  Wernecke,  Josie  and  the  Open  Inventor  Architecture  Group, 
_The  Inventor  Mentor_,  Addison-Wesley ,  Reading  Massachusetts, 
1994  . 

Advisers ;       Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Nctes:  Underwater  virtual  world  is  in  feet,  standard  DIS  PDU  is  meters 

Inventor  1.0.1  and  DIS  libraries  were  NOT  compatible  due  to 
programs  hangups  which  are  triggered  by  mallocs  in  the 
DIS  libraries  conflicting  with  the  Inventor  window. 
Removing  DIS  libraries  and  just  putting  malloc's  in  the 
Inventor  screen  redraw  callback  function  caused  identical 
program  hangups.   No  fix  was  found.   Upgrading  to  Openlnventor 
under  Irix  5.2  eliminated  this  problem  completely. 

Future  work:    Automated  camera  control 

.iv  file  load  via  WWW  URLs 

optimization 

1 1 11 1 1 1 1 1 1 1 1 1 1 1 II 1 1 1 1 1 1 1 1 II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 II 1 1 II 1 1 1 1 II II 1 1 1 

•include  ■ . . /dynamics/AUVglobals.H" 

•  include  -'iosti  eam.  h> 
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♦cncl'jde  <iomanip.h>   //  must  follow  iostream.h 
«includt-  <ecring.h> 

♦  include  <Tnach.h> 
♦include  <time.h> 

♦  include  •  gee  opt.  h"- 
♦include  <scdlib.h> 
♦include  <netdb.h> 
♦include  <netinet/in.h> 
♦include  <sys/types .h> 

1 1 1 1 11 1 1 1 1 1 ! I II / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1  n  n 1 1 1 1 1 1 1 1 1 1 1 1 1 / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

II    DIS  includes.   See  Makefile  for  other  DIS  #include  files;  they  must  match. 

♦  include  " .  . /DIS .mcasc/h/disdef s . h" 

♦  .ncludr-  "  .  .  'DIS.iTicast.^h/ appearance  .h" 

.' /  Old  rilS  include  statements  follow,  disregard... 

/  ■■  ♦include  " /n/dude/work/brutzman/DIS  .mcast/h/disdef  s  .  h" 
:  I    * include  " /n / dude /work/brutzman/DIS.mcast/h/ appearance .h" 

■  /  Jch)'.  Locke's  broadcast  version 

/.  ♦include  " /n/gra\'y  1 /work/simnet /DIS-2  .  0/h/disdef  s  .  h" 

•■/  ♦include  "  /n/grawl  /work/simnet /DIS-2  .  0/h/appea ranee  .h" 

/   Macfdrr.i  a /Zeswi  t  z  versions 

♦  :;■.: led-  "  /r. /elsie/workB  /macedoni /net /mcast/ network /h/disdefs  .h" 
♦^nclude  " /n/elsie/workB /macedoni/net/mcast/network/h/appearance .h" 

♦  '  ♦..nriude-  " /n/eisie/wcrk3 /macedoni/net/mcast/network  .  round/h/disdef  s  .  h" 

/  ■■  ♦include  "  /n  'eisie/work3  /macedoni/net/mcast/network.  round/h/ appear ance.h" 

extern  "C  (  prmtPDU  (char  •  j  ;  };  //  function  prototype  provided  for 

//  compatibility,  missing  from  DIS  library 

:;  1 1 1 1 1 1 1 1  ■  i  I  u  1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

♦  include  <x: 1 /Intrinsic .h> 

♦  include  <XiTi/Xni.h> 

♦  include  <yjTi/CascadeSG.  h> 

♦  include  '-."-Im/Foxm  .n> 

♦  include  ••Xtt  /RowCclumr. .  h~> 
«:ri elude  --/j;./  PusnE.  h> 

♦  include  <Xin/PushBG  .h> 

♦  include  --Xiri/Separator .  h> 

♦  include  <XjTi/SeparatoG  .h> 

♦  include  -'XTr:/ToggleB  .  h- 

♦  include  <XjrVToggleBG  .h> 

♦  include  •'Inventor/So .  h> 

♦  include  <-Inventor/SoDE  .h> 

♦  include  -rlnventor/Xt /SoXt .  h> 

♦include  <Inventor/Xt /SoXtPrintDialog . h>  //  see  SoOf f screenRender 

♦include  <Inventor/Xt/SoXtRenderArea .h> 

♦include  <Inventor/Xt/viewers/SoXtExaminerViewer .h> 

♦include  <Inventor/sensors/SoTimerSensor .h> 

♦include  < invent or /actions/SoCallbackActi on .h> 
♦include  <Inventor/actions/SoWriteAction.h> 

♦include  <I nventor /engines/ SoCa Icul ator .h> 
♦include  <Inventor/engines/SoElapsedTime. h> 
♦include  <Inventor/engines/SoTimeCounter .h> 

♦include  <Inventor/nodes/SoComplexity .h> 
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♦include  <Inventor/nodes/SoCone .h> 
♦•include  <Invencor /nodes/ £oCoordinate3  .  h> 
♦include  <Invencor/nodes/SoCube .h> 
♦include  <Invencor/nodes/SoCylinder .h> 
♦include  -Inventor/nodes/SoDirectionalLight .h> 

♦  include  •■-Inventor/nodes/SoDrawScyle.  h> 

♦  include  <-Inventor/nodes/SoFaceSec  .  h> 
♦include  < Invent or /nodes /SoGroup . h> 
♦include  -Invencor/nodes/SoMacerial .h> 

♦  include  v-j  nven  tor /node? /SoNurbsSur  face  .h> 
♦include  <Invencor/nodes/SoPerspectiveCamera . h> 
♦include  <Inventor/node3/£oPickStyle . h> 
♦include  < Inventor/ nodes /SoRotationXYZ. h> 
♦include  <Inventor/nodes/SoScale. h> 

♦  include  •' I nven t or / nodes /SoSe lection.  h> 

♦  include  «.  I  nven  tor/ nodes /SoSepara  tor  .  h> 
♦include  < I nven tor /nodes /SoSphere .h> 

♦  include  •- 1  nven  tor /nodes /SoSwitch  .  h> 
♦include  <Inventor/nodes/SoTransf orm. h> 

♦  include  --Inventor/nodes/SoTransf  ormSeparator  .h> 
♦include  - Inventor/nodes/SoTranslation . h> 

♦include  <Inventor/nodes/SoTriangleStripSet .h>  //  order  matters  here 
« ^ncl ude  vlnventor/nodes/SoUnits . h> 

//  leftovers  from  conversion  1.0.1  =>  Openlnventor 

//  ♦include  <Inventor/Xt /SoXtColorEditor . h>   //  not  found! 

■■  /  ♦.include  -rlnventor /nodes /SoBase.  h> 
//  ♦includtr  <Inventor /nodes  ^SoCallbackList  .h> 
'  •■■  ♦include  <Inventor /nodes/Solnteraction  . h> 
//  ♦include  <Inventor/Xt /SoXtFlyViewer . h> 
//  ♦include  <Inventor/Xt /SoXtPlaneViewer .h> 
//  ♦include  <Inventor /Xt/SoXtWalkViewer .h> 

!'!!  I  1 1 1  ,'!■'  I  i  '!'  f  I  1 1  /  1 1 1 1 1  1 1 1 1  !  1 1 1 1 1 1 1  n  1 1  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
■I    function  prototypes 

j^tiit.,;:  i:. t:   r.'I£_riet_opei'i    (;  ; 

static  vcid  DI£_net_close   ( ) ; 

static  void  DIS_Redraw_Callback  (  void     *  unused_data, 

SoSensor  •  unused_calling_sensor  ) ; 

!  I !  I! 1 1 1 1 1 : 1 1  n  I !  n  I ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

♦define   PI     3.1415926535897932 

♦define   METER£_PER_FT   0.3048 
♦define   FT_PER_METER£   3.2808 

//  utility  function  prototypes 

double   sign       (double  x) ; 

double   degrees     (double  x) ;  //  radians  input 

double   radians    (double  x) ;  //  degrees  input 

double   arcclamp   (double  x); 

double   dnormalize  (double  angle_radians) ;   //  returns  0..2PI 

int      inormalize  (double  angle_radians)    //  returns  0..359 

(return  (int)  (degrees  (angle_radians)  +0.5)  %  360;} 
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SoSeparator  *  readFi le ( const  char  'filename);  //  Inventor  Mentor  p.  284 

void     initiali ze_globais  ( ) ; 

vo:;:  parse_cominand_l ine_f lags    (int   argc,    char    '*    argv); 

I  n  !  I  i !/ 1 1 1 1 11 1 i I ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1/ / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

II  global  data  (referenced  in  callback  routines,  thus  defined  here) 


ScStparator 
SoTransform 


*  root ; 

*  AUV_posi tion_node; 


//'  initializers  needed  to  avoid  startup  segmentation  fault 


ScP.otat  ionXYZ 
SoRotar  ionXYZ 
SoRotationXYZ 


*  rotate_AUV_2  =  new  SoRotationXYZ; 

*  rotate_AUV_y  =  new  SoRotationXYZ; 
'  rotate_AUV_x  =  new  SoRotationXYZ; 


SoRotationXYZ      * 

SoRotationXYZ      * 

SoRotationXYZ 

ScRct at ionXYZ 

SoCor.e 

» 

SoCcne 

» 

SoCone 

* 

SoCone 

* 
* 

w  ^^  .  ^  ;'.  t- 

SoCone 

* 

SoSw: ten 

• 

ScSw: ten 

tt 

r  ->  C  -^^  .  -  .-  (- 

* 

ScSv.tch 

* 

SoTransf  onri 

« 

SoTransfom. 

* 

SoTransform 

* 

ScTransf  orrr. 

* 

SoTransform 

* 

ScTransfonri 

* 

ScTransf  orrr, 

* 

SoTransf  orrr, 

•K 

ScTransf  orrr. 

* 

SoCon<^ 

* 

SoMaterial  • 

si  Iver 

EoMaterial  * 

gold 

ScKaterial  * 

bras? 

SoMaterial  * 

chrome 

SoMaterial  * 

npsblue 

SoMaterial  ' 

sea green 

SoMaterial  • 

darkgree 

SoMaterial  * 

sonar_re 

r o  t  a  t  e_AUV_bow_rudders 
rotate_AUV_stern_rudders 
rotate_AUV_bow_planes 
rotate_AU\'_stern_planes 

thrusterWakePv' 

thrusterWakeAV 

thrusterWakeFH 

thrusterWaceAH 

conePi'opel  lerWakeStbd 

coneProptllerWakePort 

whichWakeFV 
whichWakeAV 
whichWakeFH 
whichWakeAK 

topsideBow 
topsideStern 
bottomsideBow 
bottom^sideStern 
lef tsideBow 
lef tsideStern 
rightsideBow 
rigntsideStern 

xfConeSonarSTlOOO 
coneSonarSTlOOO 


new  SoMaterial 
new  SoMaterial 
new  SoMaterial 
new  SoMaterial 
new  SoMaterial 
new  SoMaterial 


new  SoRotationXYZ 
new  SoRotationXYZ 
new  SoRotationXYZ 
new  SoRotationXYZ 


new  SoCone 
new  SoCone 
new  SoCone 
new  SoCone 
new  SoCone 
new  SoCone 


new 

SoSwitch 

new 

SoSwitch 

new 

SoSwitch 

new 

SoSwitch 

new 

SoTransform 

new 

SoTransform 

new 

SoTransform 

new 

SoTransform 

new 

SoTransform 

new 

SoTransformi 

new 

SoTransform 

new 

SoTransform 

new  SoTransform; 
new  SoCone; 


♦define  CAMERA_FREE  0 
•  define  CAMERA_T0_AITV  1 
•define   CAMERA_FROM_AUV  2 

static  int  whichCamera 

static  mt  PRINTDIALOG 

static  int  BACKGROUNDCOLORDIALOG 

static  int  AUTOCAMERACONTROL 


=  CAMERA_FREE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 


//  default  camera 
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szatiz   ir.t 


"EXTURE 


FALSE; 


£oF^r -pt-ct  1  veCamera 
EoPerspect:  veCarriera 
ScPerspec L i veCamera 


PerspectiveCameraFree 
Per spec  t  i  veCameraToAUV 
PerspectiveCanieraFromAUV' 


new  SoPerspectiveCamera 
new  SoPerspectiveCamera 
new  SoPerspectiveCamera 


//  ELVecjf  cameraToAUVPosation; 


SLVecJf 
SLVV-.r>f 
£bVe:3: 


behindAl'VPosj  tion 

aheadOfAUVPosiCion 

p r  1  o  r A'Jv  Position 

currentALVPosi  tion 


SbVecJf    priorCameraPosi tion ; 

SbVecJf  currentCameraPosi tion; 
SoVecjf   prjorCairieraOf  f  set ; 

£DVec3:  currentCameraOf f set ; 

£C'Vec?f  orientationPctati  onAxis ; 

f^oat  orientationRotati onAng I e ; 

rt:V,^->:  ^randardCarr-traOf f set        =  SbVecJf  (  -  20.0,  0.0,  2.0); 

float  standardCameraFocalDistance  =  20.0; 


cOiepa 

static 


'a tor    *    coiTijriand_I  ine_node; 
ciocK    t 


Dryjt 


s  t  a  t :  - 


:at:c 


SoUnits 

u-trLct  in_addr 
struct  hostent 


time_stamp_of_current_PDU, 
t  iine_of  _PDU_receipt , 
current_ciock, 
deIta_ciock; 
del  ta_tirr,e; 

PDU_Gverdue; 

Dl£_pcrt_open; 

port   !  6  ? ; 

gro-jp'  i  30  j  ; 

•  unitsfeet; 
inaddr ; 

•  hp; 


1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1  i  I N 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 i  1 1 1 1/ 1 1 1  n  1 11 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 


n 1 1 1 1 1 1 1 1 1 1 1 1 1  i  11 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

II   all  NPS  AUV  dimensions  here  in  inches 

♦define  HULLBODYLENGTH  54.00 
♦define  HULLBODYWIDTH  16.50 
♦define  HULLBODYHEIGHT    10.0 


♦define  SEAM 
♦define  STERN 
♦define  LEFT 
♦define  RIGHT 
♦define  TOP 
♦define  BOTTOM 


♦define  DOMECONTROLPT     25.00 
♦define  DOMECONTROLPTHALF  20.00 


-27.00 

// 

-43.50 

// 

8.25 

// 

-8.25 

// 

5.00 

// 

-5.00 

// 

HULLBODYLENGTH  /  2.0 

HULLBODYLENGTH  /  2.0  -  16.5 

HULLBODYWIDTH  /  2.0 

HULLBODYWIDTH  /  2.0 

HULLBODYHEIGHT  /  2.0 

HULLBODYHEIGHT  /  2.0 

//  nosecone  NURBS  surface  cooordinates 
//    offsets  to  help  define  shape 
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*d^fin^  T0FHA:.F 
*define  BOTTOMHALF 
♦define  LEFT_HALF 
*j^f;n»^  FIGKTHALF 
*defjn.r  CENTER 

♦define  FINLENGTH 
♦define  FINWIDTH 
tdefine  FINHEIGHT 


4  .75 
-4.75 

8.00 
■8.00 

0.00 

6.00 
0.75 
6.75 


//  fins  need  to  be  tapered 

//  fin  thickness  1"  at  bottom,  0.5"  at  top 


♦define  FINOFFEETFORWARD   24.0 

♦define  FINOFFSETAFT      -33.0 

♦define  FINOFFSETLEFT      12.10 

♦define  FINOFFSETRIGHT    -12.10 

♦define  FINOFFSETUP         8.875 

♦define  FINOFFSETDOWO      -8.87  5 


//  (HULLBODYWIDTH   /  2) 
//  (HULLBODYHEIGHT  /  2) 


+  (FINHEIGHT  /  2) 
+  (FINHEIGHT  /  2) 


.5' 


♦define  THPUSTERID 

♦define  THFUSTEROD 

«aefine  THRUSTERFORWARDV 

♦  define  THR'JSTEPJ\FTV 

♦define  THRUSTERFORWARDH 

♦define  THRUSTERAFTH 

idefme  SHAFTOFFSETLEFT 

«iie:ine  SHAFT0FFSETR:GHT 


3.0 
3.5 
13.0 
21  .0 
19.0 
26.0 

3.75 
-5.75 


//  forward  SEAM 

//  SEAM  *    6 

//  forward  SEAM 

//  SEAM  +  1 


14 


/  /  reverify  tnese  dimen; 
♦  jtrfme  CARDCAGELEFT 
»>define  CARDC.AGERIGHT 
♦define  CARDCAGELENGTH 
« define  CARDCAGEWIDTH 
•def.ne  CARDCAGEHEIGHT 


ions  after  rebuild 


4 
-4 

12 
7 
6 


00 
00 
00 
00 
00 


'.'  profiling  sonar,  1  degree  conical 
,' .  warm  cut  feet  ->  inches  m  code 

♦define  AUV_ST1 O00_x_of f set   34.5    //  forward  SEAM  +7.5" 
♦define  AUV_ST1000_y_off set   -2.00   // 

♦  define  Airv'_STl  00  0_z_of  f  set    4.00   //  bottom  forward 

//  scanning  sonar  sector,  1  degree  wide  by  24  degree  vertical 

♦define  AUV_ST7  2£_x_of f set  31.5    //  forward  SEAM  +4.5" 

♦  define  AU\"_ST725_y_offset  -2.00   // 

♦  define  ALr;_ST7  25_z_of f set  -4.00   //  top  aft 

:  1 1 :,'  1 1 :'!  i  1 1 1  hi  1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

FoEeparaeor  ■•  makeAliV  () 
1^ 

rotate_A'J\'_2->angle  .  setValue  (   AUV_psi); 

rctate_A'JV'_z->  axis  .  setValue  (SoRotationXYZ:  :Z)  ; 

rotate_AUV_y->angle. setValue  (-  AUV_theta); 
rotate_AUV_y->  axis . setValue  (SoRotationXYZ: :Y) ; 

rotate_AUV_x->angle. setValue  (   AUV_phi); 
rotate_AUV_x->  axis . setValue  (SoRotationXYZ: :X) ; 

SoDrawStyle  "wires; 
wires  =  new  SoDrawStyle; 
wires->style  =  SoDrawStyle: : LINES; 

11 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
II     NPS  AUV  hull  body  center  is  at  (0.0  0.0  0.0], 
//  volumetric  center  of  buoyancy 

//  fin  transformations  forward 
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SoTransform  'xfl  =  new  SoTransform; 

xf l-^translation.setValuef  FINOFFSETFORWARD,  0.0,  FINOFFSETUP) ; 

SoTransfor^n   *xf2    =   new  SoTransfonri; 

xf2->translat:ion.secValue(  0.0,  0.0,  FINOFFSETDOWN  -  FINOFFSETUP); 


SoTransform  *xf3  =  new  SoTransform; 

xf  3--^translation  .setValuei  FINOFFSETFORWARD,  FINOFFSETLEFT,  0.0); 


SoTransform   *xf4    =   new  SoTransform; 
xf4->translation.set:Value(    0.0,    FINOFFSETRIGHT 


FINOFFSETLEFT,  0.0); 


/.■■  fin  cransf  onnat:  ions  aft 

SoTransform  *xf5  =  new  SoTransform; 

Xf 5-:-tr3nslaCion.setValue{  FINOFFSETAFT,  0.0,  FINOFFSETUP); 

SoTransform  *xf6  =  new  SoTransform; 

xf 6->cranslation.secValue(  0.0  ,  0.0,  FINOFFSETDOWN  -  FINOFFSETUP); 

SoTransform  'xf  =  new  SoTransform; 

xf~--translation.setValue(  FINOFFSETAFT,  FINOFFSETLEFT,  0.0); 

SoTransform  'xfS  =  new  SoTransform; 

xfe---cransiaCion.setVaIue(  0.0,  FINOFFSETRIGHT-  FINOFFSETLEFT,  0.0) 

/•/   90  degree  increment  rotations  -  get  idefme'd  PI  values 


Sc:Fctat:cnXYZ  "ro^Ox  =  new  SoRotationXYZ 
ro9:.'x->angle.setValue  (3.141592653  /  2.0 
rcyOx->  axis.setValue  (SoRotationXYZ : :X) 

SoRotationXYZ  *ro90y  =  new  SoRotationXYZ 
rc90y-^angie.setYaIue  (3.141592653  /  2.0 
ro9C;y->  axis  .  setVaiue  (SoRotatioriXYZ  :  :  Y) 

SoRotationXYZ  *ro9Cz  =  new  SoRotationXYZ 
rT.'^''t---a:^c2e.setV5::ue  (3.141592653  /  2.0 
rcJ.-.z-  ■  ix.s  .  setValue  (SoRotationXYZ  ::  Z) 


SoRotationXYZ  TolSOx  =  new  SoRotationXYZ 
rcleL'x-^angie.setValue  (3  .  141592653)  ; 
rolBCx--  axis . setValue  (SoRotationXYZ ; :X) 


-^n.XYZ  'rclfc'j-  =  new  SoRotat.  onXYZ 
•angle. set Value  ( 3 . 14 1 592  65  3  •  ; 
•  axis. setVaiue  (SoRotationXYZ : :Y; 


SoRotationXYZ  *rol802  =  new  SoRotationXYZ 
roiSOz- -angle. setVaiue  (3  .  141592653 ) ; 
rol802->  axis. setVaiue  (SoRotationXYZ : :Z) 

SoRotationXYZ  •ro270x  =  new  SoRotationXYZ 
ro270x->angie. setVaiue  (-  3.141592653  /  2 
ro27Cx-^  axis. setVaiue  (SoRotationXYZ: :X) 

SoRotationXYZ  ^02707  =  new  SoRotationXYZ 
ro270y->angle. setVaiue  (-  3.141592653  /  2 
ro270y->  axis . setVaiue  (SoRotationXYZ: :Y) 

SoRotationXYZ  •ro2702  =  new  SoRotationXYZ 
ro2702->angle. setVaiue  (-  3.141592653  /  2 
ro2702->  axis. setVaiue  (SoRotationXYZ: : Z) 


0) 


//  construct  NPS  AUV  hull  body 

SoCube  'hull  =  new  SoCube; 

hull->width  =  HULLBODYLENGTH; 

hull->height  =  HULLBODYWIDTH; 

hull->depth  =  HULLBODYHEIGHT; 

//  construct  a  control  fin 
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SoCube  *fin  =  new  SoCube; 

fir,-vwidih  =  FINLENGTH; 

fin--'heiaht  =  FINWIDTH; 

f:n->deprh  =  F INK EIGHT, • 

■',  CO!:;?!:  ruct  f:n  pa:rs 

rotace_AUV_bow_rudders  =  new  SoRotationXYZ; 
rotate_AUV_bow_rudders->axis .setValue  (SoRotationXYZ : :Z) ; 
rotate_AUV_bow_rudders->angle. setValue  (  0.3  ) ; 

rotate_AUV_stern_rudders  =  new  SoRotationXYZ; 
rotate_AUV_stern_rudders->axis .setValue  (SoRotationXYZ: :Z) ; 
rotate_AUV_stern_rudders->angle. setValue  (  0.6  ) ; 

rotate_AUV_bow_planes  =  new  SoRotationXYZ; 
rotate_AUV_Dow_pIane£->axis. setValue  (SoRotationXYZ: :Z) ; 
rotate_AUV_bow_planes- >angle . setValue  (  0.9  ); 

rctat.^_A'JV_stern_planes  =  new  SoRotationXYZ; 

:ot  jte_Ai'V_stern_pianes->5xis  .setValue  (SoRotationXYZ:  :Z)  ; 

i"otate_AUV_stern_planes->anglfe .  setValue  (  1.2  ); 

//  construct  forward  vertical  fins  (bow  rudders) 

SoSeparator  *fvfins  =  new  SoSeparator; 

fvf :n£- -addChild (  xfl  ;; 

fvf  ins--  -adi^hi  Id  '    rotate_AUy_bow_rudders    ); 

f  viaTiS- -adiicnild  I    fin    i; 

fvf ins->3ddcnild I  xf2  ); 

fvf  :r.r->addCh:Id  (  roISCx  1;     //  net  rotation  180 

fvf :ns->addCh:id  ;  fin  ); 


! I   construct  aft  vertical  fins  (stern  rudders) 
SoSeparator  'avfins  =  new  SoSeparator; 

avfins-  -addrhiid (  xf5  ); 

avf  in.--  -addC-.ild  ■  rotate_AUV_stern_rudders  ); 

avians- -addCn..  ;Ld  ,  fin  ); 

avf  jns- .-addCni  Id  i  xf6  ); 

avf :ns->addCnild (  roieOx  );     //  net  rotation 

avf ins->addChild (  fin  ); 


180 


//  construct  forward  horizontal  fins  (bow  planes; 
SoSeparator  '•fhfins  =  new  SoSeparator; 
fhfins->addChild(  xf3  ); 

(  ro90x  ) ;      //  net  rotation 

(  rotate_AUV_bow_planes  ) ; 

(  fin  ); 

1  ro27  0x  ) ; 

(  Xf4  ) ; 

(  ro270x  ) ;      //  net  rotation 


fhfins->addChild 
fhfins->addChild 
fhf ins->addChild 
fhf ins->addchild 
fnf ins->addChild 
f  hf  j.ns->addChild 


90 


270  (in  case  of  fin  asymmetry) 


fhf :ns->addChild (  fin  ); 


//  construct  aft  horizontal  fins  (stern  planes) 

SoSeparator  *ahfins  =  new  SoSeparator; 

ahfins->addChild(  xf7  ); 

ahfins->addChild  (  ro90x  );      //  net  rotation   90 

ahf ins->addChild (  rotate_AUV_stern_planes  ); 

ahfins->addChild (  fin  ); 

ahfins->addChild(  ro270x  ); 

ahfins->addChild(  xf8  ); 

ahfins->addChild(  ro270x  );    //  net  rotation  270 

ahfins->addChild(  fin  ); 


[in  case  of  fin  asymmetry) 


//  construct  cylinders  to  represent  the  thrusters 

SoTransform  *xfl3  =  new  SoTransform; 

xf 13->translation.setValue(THRUSTERF0RWARDV,  0.0,  0.0); 
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SoTransform  •xfl4  =  new  SoTransf orTn; 
xf  14->translat:on  .  setValue  (THRUSTERAFTV,  0.0,  0.0); 
SoTransf orrr,  *xfl5  =  new  SoTransform; 

xf  15->t:ranslation.secValue(THRUSTERF0RWARDH,  0.0,  0.0); 
ScTransforTTi  •xfie  =  new  SoTransform; 
xf 16-  -translation. setValue (THRUSTERAFTH,  0.0,  0.0); 

SoC\linder  •  tubeV  =  new  SoCylinder; 
tubeV->radius  =  THRUSTERID; 
ti.beV->.nei3ht  =  Hl-'LLBODYHEIGHT  +  0.2; 
SoO/linder  *  tubeH  =  new  SoCylinder; 
turjeH- >radius  =  THRUSTERID; 
tJDeH->height  =  HULLBODYWIDTH   +  0.2; 

SoCcmpIexicy  •  wakeComplexity  =  new  SoComplexi ty ; 
wakeComplex: ty->value  =  0.2; 

/.'  still    inches 

topslde&ow->  translation .setValue (  0,    TOPHALF  +  AUV_bow_vertical ,  0); 

botto.n.-£..deBow->  translation  .  setValue  (  0,  BOTTOMHALF  +  AUV_bow_vertical ,  0); 
bottoms:de3ow-:-     rotation .setValue (SbVec3f  (  0.0,  1.0,  0.0  ),  M_PI  ); 

topsideStern->  translation. setValue (  0,  TOPHALF  +  AUV_stern_vertical , 0) 
bcttorr.s:d€S:ern->translation.setValue(  0,  BOTTOMHALF  +  AUV_stern_vertical ,  0) 
Dcttorr.s.aeStern--.-   rotation .  setValue  (SbVec3f  (  0.0,  1.0,  0.0),  M_PI  ); 

itf tside&ow->    translation. setValue (  0,  LEFT_HALF  +  AUV_bow_lateral ,  0); 
leftsidfrBow->       rotation .setValue (SbVec3f  (  0.0,  1.0,  0.0  ),  M_PI  ); 
r:gr.tsideBow->    translation  .setValue  (  0,  RIGHTHALF  +  AUV_bow_lateral ,  0); 

leftsid6St€rn->  translation .setValue (  0,  LEFT_HALF  +  AUV_stern_lateral ,  0); 
leftsideStern->     rotation . setValue (SbVecBf  (  0.0,  1.0,  0.0   ),  M_PI  ); 
rightsideStern---  translation .  setValue  ;  0,  RIGHTHALF  *  AU\'_stern_lateral ,  0); 

thr-jn-^rWakeFv"  =  new  SoCcne;  //  global  for  callbacks 
tr.rustei'WaKeFv- ^."-jeight       =  AU\'_bow_vertical  *  2.0; 
thrusLerWakeFV->bottomRadius  =  ALT^.'_bow_vertical  /  4.0; 
thr'-:r:e:-Wakerv'->part£        =  SoCone:  :SIDES; 


whirhWa 
wnichwak 
wnicnwa 
w.-.:c.hWi 


eF'.'  -    new  SoSwitcn; 

eFV->addChild  (topsideBow) ; 

erV->addCnild  (DcttomsideBow; ; 

eFV- -.w.hj  chChild  =  G;  //  default  topsideBow 


SoStz-p^:  iter  *  thrusterFv'  =  new  ScSeparator; 

tnrust^iF;- ^addC.Mid  :  xflB  ); 

thrusterFV-->addChjld(  ro90x  ) 

thrusterFV->addcnild(  tubeV  ) 

thrusterFV->addChild (  wires  ) 

thru3terFV-:>addChild  (  wakeComplexity  ); 

thiustti  F.'- -addChild  (  seagreen  j; 

thrusterF^->addChild (  whichWakeFV  ); 

thrusterFV->addChild (  thrusterWakeFV  ); 

thrusterWakeAV  =  new  SoCone;  //  global  for  callbacks 
thrusterWakeAV->height       =  AUV_stern_vertical  *  2.0; 
thrusterWakeAV->bottomRadius  =  AUV_stern_vertical  /  4.0; 
thrusterWakeAV->parts        =  SoCone: : SIDES; 

whichWakeAV  =  new  SoSwitch; 
whichWakeAV->addChild  (topsideStern) ; 
whichWakeAV->addChild  (bottomsideStern) ; 
whichWakeAV->whichChild  =  0;  //  default  topsideStern 

SoSeparator  •  thrusterAV  =  new  SoSeparator; 

thrusterAV->addChild(  xfl4  ); 
thrustexAV->addChild(  ro90x  ); 
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:r.i-JSterAV--^addChild  '  cubeV  )  ; 
t;hrL:sterAV->addChiid  f  wires  )  ; 
Lhrusce:AV->addCnild I  wakeComplexity  ,; 
thrusLerAV->addChiId 1  seagreen  ); 
thrjsterAV->addChild I  whichWakeAV  ); 
i;.i-^i;teiAV-  -addCh:  id  (  thrusterWakeAV  )  ; 

chrusterWakeFK  =  new  SoCone;  //  global  for  callbacks 

thru3t:erWakeFH->height  =  AUV_bow_lateral  *  2.0; 

thrusterWakeFH->bottomP.adius  =  AUV_bow_lateral  /  4.0; 

thrusterWakeFH->parts  =  SoCone: :£IDES; 

whichWakeFH  =  new  SoSwitch; 
whichWakeFH->addChild  (lef tsideBow) ; 
w.MchWakeF.H-i-addChild  (rightsideBow)  ; 
whichWakeFH->whichChild  =  0;  //  default  leftsideBow 

SoSeparator  *  thrusterFH  =  new  SoSeparator; 

tnrL:sterFH->addChiid  ;  xfl5  ); 

chjrusterFH--'addChild',  ro90y  ); 

!:hrusterFH->addChild(  CubeH  ); 

i;";ru;?c-^rFH- -addChiid  i  rol8Cz  ); 

t!'.rusterFH->addCha  id  (  wires  i; 

tnrusterFH->addChiid (  wakeComplexity  ); 

thrusterFH->addChild (  seagreen  ); 

thrusterF.H-->addCniid  (  whichWakeFK  ); 

thrust.^rFH->addChild  (  thrusterWakeFH  ); 

thrusterWarceAK  =  new  SoCone;  //  global  for  callbacks 
t.'-.r'-;jt:erWai'.eAH->heign:       =  AUV_stern_iateral  *  2.0; 
thrLs:eiWar:eAH->bot tomRadius  =  AUV_stern_lateral  /  4.0; 
thrusterWakeAH->parts        =  SoCone: :£IDES; 

whichWakeAH  =  new  SoSwitc.h; 

wh2chWakeAH->addChild  (lef tsideStern) ; 

whichWakeAH- >addCniid  (rightsideStern) ; 

wh: rhWakcAH- ^whicnCh-ic  =  0;  //  default  lef tsideStern 

SoSeparator  *  thrusterAH  =  new  SoSeparator; 

thrusterAH->addChild (  xfl6  ); 

thi-usterAH->addChiid  (  ro90y  ); 

tnrusterAH-  -addcniid (  tubeH  j; 

thruSterAH->addChild (  rolSOz  ); 

tnrusterAK->addChiid (  wires  ); 

thrusterAH->addChi id (  wakeComplexity  ); 

thrusterAH->addChild (  seagreen  ); 

tnrusterAH->addChiid(  whichWakeAH  ); 

thrusterAH->addChiid (  thrusterWakeAH  ); 

//  construct  internal  CARDCAGEs  left  and  right 

SoCube  'cardcagelef tbox  =  new  SoCube; 

cardcaaelef tbox->width   =  CARDCAGELENGTH ; 

cardcageleftbox->height  =  CARDCAGEWIDTH; 

cardcageleftbox->depth   =  CARDCAGEHEIGHT; 

SoTransform  *xfcardcagelef t  =  new  SoTransform; 
xfcardcageleft->translation.setValue(  0.0,  CARDCAGELEFT,  0.0  ); 

SoSeparator  *cardcagelef t  =  new  SoSeparator; 
cardcageleft->addChild  (  xf cardcagelef t  ); 
cardcageleft->addChild{  cardcagelef tbox  ); 

SoCube  *cardcagerightbox  =  new  SoCube; 

cardcagerightbox->width  =  CARDCAGELENGTH; 

cardcagerightbox->height  =  CARDCAGEWIDTH; 

cardcagerightbox->depth  =  CARDCAGEHEIGHT; 
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SoTransf orrri   'xfcardcageright   =   new  SoTransform; 
xfcardcagerighi:->t:ransIation.setValue  {    0.0,    CARDCAGERIGHT,    0.0    ); 

SoSeparator  *cardcagerighc  =  new  SoSeparacor ; 
cardcageright:->addChild  (  xfcardcagerighc  )  ; 
cardcageright->addChiid (  cardcagerightbox  ) ; 


11  n  11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1!  1 1 1 1 11 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 11 1 1 1  / 1 1 1 1 1 1 1 1 1 1 11  / 1 1 
1 1   construct  main  body  out  of  parts 
SoGroup  'body  =  new  SoGroup; 

body-->addChi]d  (  gold  )  ; 

body ->addChi id ;  fvf:ns  ) 

bod!>-->addChiid !  avfins  ) 

bcdy'-^addChild :  fhfins  i 

bodv---addChild  (  ahfins  ) 


body---addCr;i  ]d  :  npsblue  )  ; 
h '>d-.  -  -addTI;:  Id  :  .huil  )  -, 


body-  -addChild 
body-  -addCn:  id 
bcd>'->addCn:  id 
bcd-y-  ^addC.^.iid 
bcdy-->addCh:  Id 
body->addChi  id 
bcdy->addChi id 


si Iver  ; ; 
thrusLerFv'  ) 
thruSterAV  ) 
thrusrerFH  ) 
thru?rerAH  ) 
cardcagelef t 
cardcagerjght 


1 1 1 1 1 1 !  I  ! 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 !:: J  1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
It    ccnsrruct  nosecone  using  a  NURBS  surface 


static  float  pts 


i\- 


) 


-  SE.Af-:, 

-  SEAJ-i, 

-  SEAM, 

-  SEAM, 

-  SEAJ-:, 

-  SE.A>; 

-  SEA.V 

-  SEAM  ■ 

-  SEAI-^ 

-  SEAl-:, 

-  SEAM 

-  SE.A>: 

-  SE.AJ-: 

-  SEAJ-1, 

-  SEAM, 

-  SEAM 

-  SEAM  ■ 

-  SEAM 

-  SEAM, 

-  SEAM, 

-  SEAM, 

-  SEAM, 

-  SEAM, 

-  SEAM, 


DOMECCN'TROLPTHALF , 
DOMECONTROLPTHALF, 
DOMECOKTROLPTHALF , 


DOMECONTROLPTHALF , 
DOMECOKTROLPT, 
DOMECONTROLPTHALF , 


DOMECONTROLPTHALF , 
DOMECONTROLPTHALF , 
DOMECONTROLPTHALF , 


LEFT,  TOP 

LEFT_HALF,  TOP 

CENTER,  TOP 

RIGHTHALF,  TOP 

RIGHT,  TOP 


LEFT, 

LEFT_HALF, 

CENTER, 

RIGHTHALF, 

RIGHT, 

LEFT, 

LEFT_HALF, 
CENTER , 
RIGHTHALF, 
RIGHT, 

LEFT, 

LEFT_HALF, 

CENTER, 

RIGHTHALF, 

RIGHT, 

LEFT, 

LEFT_HALF, 

CENTER, 

RIGHTHALF, 

RIGHT, 


TOPHALF 
TOPHALF 
TOPHALF 
TOPHALF 

TOPHALF 

CENTER 
CENTER 
CENTER 
CENTER 
CENTER 

BOTTOMHALF 
BOTTOMHALF 
BOTTOMHALF 
BOTTOMHALF 
BOTTOMHALF 

BOTTOM 
BOTTOM 
BOTTOM 
BOTTOM 
BOTTOM 


static  float  knots  [10] 
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{  0 ,  0 ,  0 ,  0 ,  0 ,  1 ,  i ,  1 ,  1 ,  :  )  ; 

SoComplexi ty  *noseconeComplexi ty  =  new  SoComplexity ; 
no?<^roneConipIexi  ty-  -value  =  0.7; 

SoCoordinat:e3  'controlPts  =  new  SoCoordinate3; 
conti"olPcs->point .  setValues  {  0,  25,  pes  )  ; 

SnNurbsEurf ace  •sonardome  =  new  SoNurbsSurf ace; 
sonardorrie- >numUConcroi  Points  .  secValue  (  5  ); 
sonardome-  »nuinVConcro2  Points  .  setValue  (  5  ); 
sonardome->uKnotVector .setValues  (  0,  10,  knots  ) ; 
sonardome->vKnotVector .setValues  (  0,  10,  knots  ); 

SoSeparator  *nosesection  =  new  SoSeparator; 
nosesection->addChild '  npsblue  ); 
nosesection->addChiid f  noseconeComplexity  ); 
nosesection->addChiId {  controlPts  ); 
ncsesection->addCnild (  sonardome  ); 

ccneSonarSTlOOO  =  new  SoCone;  //  global  for  callbacks 

ccneSonarST:000->height  =  fabs  (AU\'_ST1000_range)  ; 

coneSonar£T1000->bottoiTiRadius  =  fabs  (AUV_ST1000_range)  /  60.0;  //  1  degree 

coneSon&rST1000-->parts  =  SoCone  ::  SIDES; 

.'/  drawn  froir,  center 

x:  7';,e£cr,ar£T:  'oOi  -  >translat  ion  .  set  Value  (  0.0, 

//  -  (HULLBODYLENGTH   /  2.0) 

-  (AUV_ST1000_range/  2.0),  4.0/12.0) 


-aratrr  '  sepSonar  =  new  SoSeparator; 


sepScnar-  'addCh 


,ld'  rc90z 

sepSonai'-  -addCh:  Id  •:  wires  ); 

sepSonar->addChild .  wakeComplexi ty  .; 

sepScnar->addChiId !  sonar_red  ;; 

sepSor.ar- -addCnild  i  xf  ConeSonarSTlOOG  }; 

sepSonar- -addCni Id !  coneSonarSTlOOO  ); 

■  n  1 1  ';  I  !'■  1 1  i 1 1 1 1 1 1 i  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 / 1 1 1 
I      Define  tail  section 

/'  -^ii.-ure  polygons  are  defined  in  clockwise  fashion! 

//  Two  triangles  using  SoTriangleStripSet : 
static  long  numbertrianglevertices  [2]  =  {3,  3}; 

static  float  af ttrianglevertices  [6] [3]  = 

I 
(STERN,  LEFT,   0.0},  {SEAM,  LEFT,   TOP),     {SEAM,   LEFT,   BOTTOM), 
{STERN,  RIGHT,  0.0),  {SEAM,  RIGHT,  BOTTOM),  {SEAM,   RIGHT,  TOP), 

); 

//  Define  coordinates  for  triangular  vertices  &  SoTriangleStripSet 
SoCoordinate3  *tailcoordl  =  new  SoCoordinateB; 
tailcoordl ->point . setValues  (0,  6,  af ttrianglevertices) ; 

SoTriangleStripSet  ♦tail triangleset  =  new  SoTriangleStripSet; 
tailtriangleset->nuniVert  ices.  setValues 

(0,  2,  numbertrianglevertices); 

//  Two  rectangles  using  FaceSet: 

static  long  numberquadvertices  [2]  =  {4,  4);   //  ref .  p.  5-6 

static  float  af tquadvertices  [8] [3]  = 

( 
{STERN,  LEFT,   0.0),   (STERN,  RIGHT,  0.0),   {SEAM,   RIGHT,  TOP), 
{SEAM,   LEFT,  TOP), 
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(STERN,  RIGHT,  0.0),   (STERN,  LEFT,   0.0),   (SEAM,   LEFT,  BOTTOM), 
iSEAM,   RIGHT,  BOTTOM), 
); 

! '   Define  coordinates  for  quad  vertices  &  SoFaceSet 
SoCoordinate3  ■'tailcoord2  -   new  SoCocrdinate3  ,■ 
tai  Jcoord2->point .  setValues  (0,  8,  af tquadvertices) ; 

SoFaceSet  *tailquadset  -    new  SoFaceSet; 
taiIquad3et->nuinVertices  .  setValues  (0,  2,  numberquadvertices)  ; 

//  Two  cylinders  currently  represent  the  propellers  -  improve  this! 

//  a  much  fancier  individual  prop  model  is  possible  here,-  also  add  complexity 

SoCylander  'prop  =  new  SoCylinder; 

prop->radius  =  2.00; 

prop- -height  =  1.00; 

/;  Two  cylinders  to  represent  the  shafts 
SoC"yiinder  "shaft  =  new  SoCylinder; 
shaf t->radius  =  0.50; 
shafc->heig.'-i  =  4.00; 

SoTrar.sf orTi  *xfl8  =  new  ScTransform;   /,'  shafts  relative  to  props 

//  note:   rotated  902 
xflc->tran3lation.setValue (0 .0,  -2.0,  0.0;; 

ScTransforiTi  *xfll  =  new  SoTransform;   //  left  prop 

xf  i:->t:-an3:ation.setValue(STERN  -  2.0,  SHAFTOFFSETLEFT,   0.0); 

//  compose  shaft  with  prop 
SoSeparator  *leftprop  =  new  SoSeparator; 
lef tprop->addChild (  xfll  ); 
leftprop->addChild (  ro90z  ); 
ief tprop- >aGdCnild (  prop  ;; 
lfrftprop->addChild(  xflB  ); 
leftprop->addChild ;  shaft  ); 

SoTransforx  'xf PropellerWakePort  =  new  SoTransform; 
xf  PropeilerWaKePort->translation  .  setVaiue ',  0.0,  15.0,  0.0  ); 
SoSeparator  •  separatorPropel lerWakePort  =  new  SoSeparator; 
separatorPropeilerWaKePort->addChild i  xf PropellerWakePort  ); 
separatorPropellerWaKePort->addChild (  rolBOx  ); 
separatcrPropellerWakePort->addChild (  wires  ); 
separatcrPropellerWaKePort->addChild (  wakeComplexi ty  ); 
separatorPropellerWakePort->addChild (  seagreen  ); 
ccnePropellerWakePcrt  =  new  SoCone;  //  global  for  callbacks 
conePropellerWakePcrt->height       =       AUV_port_rpm  /  700.0  •  24.0; 
conePi  opellerWaKeFcrt-->bottomRadius  =  fabs  (Airv7_port_rpm)  /  700. C  •   6.0; 
conePropellerWakePcrt->parts        =  SoCone :: SIDES; 
separatorPropellerWakePort->addChild '  conePropellerWakePort  ) ; 
ief tprop->addChi Id  I  separatorPropellerWakePort  ); 

SoTransform  •xfl2  =  new  SoTransform;   //  right  props 

xf i2->tran3lation.setValue(STERN  -  2.0,  SHAFTOFFSETRIGHT,   0.0); 

SoSeparator  *rightprop  =  new  SoSeparator; 
rightprop->addChald(  xfl2  ); 
rightprop->addChild(  ro902  ); 
rightprop->addChild(  prop  ); 
rightprop->addChild(  xfl8  ); 
rightprop->addChild (  shaft  ); 

SoTransform  "xf PropellerWakeStbd  =  new  SoTransform; 
xf PropellerWakeStbd->translation.setValue(  0.0,  15.0,  0.0  ); 
SoSeparator  *  separatorPropellerWakeStbd  =  new  SoSeparator; 
separatorPropellerWakeStbd->addChild (  xf PropellerWakeStbd  ); 
separatorPropellerWakeStbd->addChild (  rolBOx  ); 
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separaLorPropellerWakeSLbd->addChi Id (  wires  ) ; 
separatorPropellerWakeStbd->addChild (  wakeComplexi ty  ) ; 
separatorPrcpel lerWakeStbd->addChild (  seagreen  ); 
coneFropellerWakeStbd  -   new  SoCone;  //  global  for  callbacks 
conePropellerW3keScbd->height       =       AUV_port_rpm   /  700.0  *  24.0; 
conePropellerWakeStbd->bottomRadius  =  fabs  (AUV_port_rpin)  /  700.0  *   6.0; 
conePropellerWakeStbd->parts        =  SoCone :: SIDES; 
separaCorPropellerWakeScbd->addChild (  conePropellerWakeStbd  ); 
righrprop- >addChild (  separator Propel lerWakeStbd  ); 

//  construct  stern  box  support  beneath  aft  vertical  fins 

SoCube  'sternf insupportcube  =  new  SoCube; 

sternf insupportcube->width   =  FINLENGTH  +  1.5; 

sternf insupportcube->height  =  5.0; 

sternf insupportcube->depth   =  HULLBODYHEIGHT  -  0.5; 

SoTransform  *xfl7  =  new  SoTransf orm; 

xf l''->translation.setValue(FINOFFSETAFT,  0.0,   0.0); 

SoSeparator  *sternf insupport  =  new  SoSeparator; 

sternf jnsupport->addChild I  xfl7  ); 

sternf insupport->addChild (  sternf insupportcube  ); 

ScSefarator  *taiisection  =  new  SoSeparator; 

ra:l.?er:  :c.-;- ^addChild  (  npsblue  ); 

ta:^section->addCni:d !  taiicoordl  '■  ; 

taiisec: :on->addChild (  tailtriangleset  ); 

1  i-<^?- :cn----addChild  (  ta:lcocrd2  ); 

lsection->addChild (  tailguadset  ); 

lsect-cri->addChild  (  sternf  insupport  ); 

]?^c:  :or;--addChild  >  brass  );  » 

ta.  1  Sr-c: -on- .•addC'iiid  leftprop  ,■  ; 

tail  r^tt  :on- .-addr.hi  id  !  rightprcp  }; 

1 1  .■  1 1  :,■:;:  1 1 ;:!  I !  1 1 1 1 ;  i  1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 11 1 1 1 1 1 1 1 1 
/  robot  is  just  units  i.  body  &  nosesection  u    tailsection  i  extra  stuff 

SoSeparator  '  robot  -   new  SoSeparator; 

robot- -addCnald '  unitsfeet  ); 

//  robc.:  root  transform  for  overall  vehicle  orientation 
Air/_pos_  tion_node  -   new  SoTransf  orm; 

A'.TV_Dosition_node->translation.setValue(0.0,  0.0,  0.0); 
roDct->addChild (  AUV_position_node  ); 

rot3te_A'vT\'_2  =  new  SoRotationXYZ; 
rotatt_Airv_z->angle.setValue  (  -  AUV_psi  )  ; 
rotate_AW_z->  axis  .  setValue  (SoRotationXYZ:  :Z)  ; 
robot->addCnild (  rotate_AUV_z  ); 

rotate_Airv'_y  =  new  SoRotationXYZ; 
rotate_AUV_y->angle. setValue  (  -  AUV_theta); 
rotate_AUV_y->  axis . setValue  (SoRotationXYZ : :Y) ; 
robot->addChild(  rotate_AUV_y  ); 

rotate_AUV_x  =  new  SoRotationXYZ; 
rotate_AUV_x->angle. setValue  (   AUV_phi ) ; 
rotate_AUV_x->  axis . setValue  (SoRotationXYZ: :X) ; 
robot->addChild(  rotate_AUV_x  ); 

robot->addChild(  sepSonar  );   //  feet 

SoUnits  *unitsinches  =  new  SoUnits; 
unitsinches->units .setValue  {  SoUnits :: INCHES  ); 
robot->addChild(  unitsinches  ); 
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SoPickStyle  •  unpickablestylenode; 
SoPickScyle  •  pickablescylenode; 
unpickablestylenode  =  new  SoPickStyle; 

pickabiestylenode  =  new  SoPickStyle; 
unpickablestylenode->style.setValue  (  SoPickStyle :  :UNPIC?CABLE  ); 

pickablestylenode->style.setValue  (  SoPickStyle: :BOUNDING_BOX  ); 

//  Make  subsequent  nodes  unpickable  so  that  AUV  is  treated  as  a  whole 
robot->addChald (  unpickablestylenode  ); 

roLot->addChiid (  body  ) ; 
rcr..::t->addChild  (  nosesection  ); 
robot ->addChi Id (  tailsection  ); 

ScTransform  *xfl9  =  new  SoTransf orm; 

xf 19->translation. setvalue (  0.0,  0.0,  -  2.0); 

robot->addChild(  xfl9  ); 

rcbcc---addChild  i  new  SoPointLight  ); 

ccut  <•  "new  peine  light  added  under  robot"  «  endl ; 


.■eturr,  rooot; 


'!■!:'■  I !  i :  1 1 1 1 !  I :!  1 1 1 1 1 1 1 1 1 11 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 


dc_c.t  sign  (acut^t  x; 

if       (X  >  0.0)  returj^  1  .0 

else  if  (x  <   0.0)  return  -1.0 

else             return  1 . 0 


II- 


■II 


double  decrees    (double  x)  //   radians   input 

ret.,r.-i   x    ■•    IS'  .  0    ,'    PI ; 


■/■/ 


double  radians  (double  x)    //  degrees  input 
return  x  *  PI  /  180.0; 


double  arcciamp  (double  x) 
( 

if      (X  >   1.0) 
{ 

X  =   1.0; 

cout  «    "viewer:   arcciamp  reduced  "  «  x  <<  "  to  1 .0"  «  endl, 
) 

else  if  (x  <  -1 . 0) 
{ 

X  =  -1.0; 

cout  «    "viewer:   arcciamp  raised  '  <<  x  «  •  to  -1.0"  «  endl, 
) 

return  x; 


■// 


) 

// 

double  dnormalize    (double  angle_radians) 


■// 
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double  new_anqle  =  angle_radians ; 

wh:2e  (new_angle  >  2*PI)  new_angle  -=  2*PI; 

w.-:i2»=-  ;r;ew_ar!g2e  •'  0.0  )  new_angle  -^=  2'PI; 

refjrn  new_angle; 
1 
)/! 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 


II- 


■II 


static  inc  DIS_nec_open  ()   //  Ref:  macedonia  include  files 

Multicast  Defaults  from 
//  /n/eisie/workB/macedoni  /net /incast /net  wo  rk/uti  Is/planes /planes  .cc 

//     ttl  value  does  not  matter  since  this  viewing  program  only  reads  PDUs 
u_char  ttl  =  16;         //  multicast  ttl=16  stays  inside  NPS 

//  0  =  flat  world,  1  =  round  world 


int  exercise_id  =  -1 
int  coordinate_system  =  0 ; 
char  "utmfile         =•" 


mt      result    =   net_open    (port,    group,    ttl, 

exercise_id,    coordinate_system,,    utm_file;; 

-r.t   rerult  =  net_open  (port,  group,  ttl);  //  old  version 

if    res-It  ==  FALSE' 

co-t  ■'•'  "viewer:  DIS_net_cpen  ()  failed"  <<  endl ; 

DI£_port_open  =  FALSE; 
t 
el  se 


.' /  cout  <-■  "Viewer:  port  =  "   <--  port     <<  ",  group  =  "  <<  group 

//      <^  ",  ttl  =  "      <<  ttl      «  endl; 

//  cout  <-■  '      exercise_id  =  "  «   exercise_id 

/,■■      '•  ",  coordinate_system  =  "    <<  coordinate_system 

'-•  ",  utir_file  =  \""  <^  utm_file  «  "\'"  «  endl; 


return  result; 


static  void  DIS_Redraw_Callt/ack  (  void     *  unused_data, 

SoSensor  *  unused_calling_sensor 


■// 


double  delta_x  =0.0 

double  delta_/  =  0.0 

double  delta_z  =0.0 

double  delta_phi  =  0.0 

double  delta_theta  =0.0 

double  delta_psi  =  0.0 

int 

EntitylD 
EntityType 
EntityStatePDU 
char 


number_of_PDUs ; 

UUV_DIS_id; 

UUV_DIS_type; 

*  UUV_DIS_pdu  =  NULL; 

*  local_PDU    =  NULL; 
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PDUT-ype  I  oca  l_PDU_cy  pe  ; 

static  mt  rcvd  =  0; 

char  NPSAlTV_Marking  [8]; 
bzerc  (NP£A'JV_Marking,  8); 
strncpy   (NPSAUV_Marking ,  "NPS  AUV',  7);  //  4  more  free  chars  remain 

if  (delr:a_':ime  <=  5.5) 

couc  <<  'viewer:  DIS_Redraw_Callback :  PDU  loop  delca_time  =  * 
<<  delta_timt  <<  endl ; 

while  (TRUE)   //  until  break 
( 

number_of_PDUs  =  net_read  (i  locai_PDU,  U    local_PDU_type) ;  //  old  version 

number_Oi_PDUs  =  net_read  {i  local_PDU,  &  local_PDU_type,  &  inaddr) ; 

//  cojt  •■<  "viewer:  net_read  complete,  number_of_PDUs  available  =  • 
•-•'  number_cf_PDUs  ■'■'   endl  ; 

if  injmDei_cf_PDU?  ==  -1) 

cout  •■•■  'viewer:  Error  on  net_read,  number_of_PDUs  =  -1"  «  endl  ; 

if  :njmber_of_FD'J^  ^=  0) 
1 

break,   //  no  more  PDUs ,  done  for  now 

/ ,'  rcvd  -r  +  ; 

/,■  cout  <<  'PDU  received,  rcvd  =  "  «  rcvd  «   endl; 

/;  printPDU  (:oc5i_PDUi ; 

UUw'_D:£'_pdu  =  (EntityStatePDU  •)  locai_PDU; 

//  ensure  the  PDU  values  are  the  right  types 
If  (iCca:_PDU_type  !=  EntityStatePDU_Type) 
{ 

cout  <^    "viewer:  local_PDU_type  !=  Enti tyStatePDU_T>'pe,  ignored..." 
<<  endl ; 

/  /  dc:. '  t  forget  this  or  get  a  meiricry  leak! 
//  articulated  parameters  are  also  freed 
freer-D'J  iicjiar  '»  UUV_DlS_pdu  i  ; 

cout  <--  "viewer:  freePDU  ((char  •)  UU\'_DIS_pdu )  called  for  this  PDU" 
<<  endl ; 

continue;   //  not  a  break  since  other  PDUs  may  be  waiting 
) 
else  if  '?trncmp  '(char  *)  UUV_DIS_pdu->entity_marking. markings, 

NPSAUV_Marking,  7)  !=  0) 
{ 

cout  <-•'  "viewer:  non-NPS  AUV  Entity  State  PDU  encountered,  ignored...' 

<<-   endl ; 
//  printPDU  llocal_PDU); 
freePDU  ((char  •)  UUV_DlS_pdu ) ; 
//  don't  forget  this  or  get  a  memory  leak! 
//  articulated  parameters  are  also  freed 

cout  «  "viewer:  freePDU  ((char  •)  UUV_DIS_pdu)  called  for  this  PDU" 
<r<   endl ; 

continue;   //  not  a  break  since  other  PDUs  may  be  waiting 
) 
//  cout  «  "PDU  OK"  «  endl; 

//  extract  parameters  of  an  entity  state  PDU  (most  are  listed  in  pdu.h) 
//     this  assumes  there  are  no  articulated  parameters  (add  later)  <<< 
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// 

/  / 


/  / 

// 

// 


// 
// 
// 
// 
// 


//  DI£  ID  and  Type 

//  UUV_DI£_jd    =  UUV_DlS_pdu->enLity_id; 
//  UUV_DIS_cype  =  UUV_DIS_pdu->enti ty_type; 
t:Ime_scamp_of_current_PDU  =  UUV_DIS_pdu->entity_state_header . time_stamp; 
c:n'ie_of_PDU_receipt       =  clock  (  )  ; 


PDl'_overd'je   =    FALSE; 

//    Posture 

AUV_x 

A\JV_y 

Air\'_2 

AlJV_phi  =    radians 

AU\'_cheta  =    radians 

A'J'v'_psi  =    radians 


UUV_DIS_pdu->entity_location.x  *  FT_PER_METERS; 

UUV_DIS_pdu->Gntity_location.y  *  FT_PER_METERS; 

UUV_DIS_pdu->entity_location.z  *  FT_PER_METERS; 
(UUV_DIS_pdu->entity_orientation .phi ) ; 
(UUV_DIS_pdu->entity_orientation.theta) ; 
(UUV_DIS_pdu->entity_orientation.psi ) ; 


couc  <•< 
cout  << 
cout  << 
couc  << 

couC  << 
couc 
cout 
couc 


lewer:  DIS_nec_read  posture  trace:"  <<  endl ; 


<< 
<< 


UUV_DIS_pdu->entity_location .x 
UUV_DI S_pdu - >enc i cy_l ocaci on .y 
UU'v_DIS_pdu->enCicy_iocacion.  z 
UlJV_DI£_pdu->entity_orientation.phi 
UUV_DI£_pdu->enticy_orientation. theta 
UUV_DI £_pdu - >en 1 1 ty_or  i  ent  a  t  i  on . ps  i 


<< 
<< 

<< 
<< 
<< 

<<    • ] •    <<  endl ; 


/ /    Linear  and   angular   velocities    in  body   coordinates/meters  by   DIS   standard 

Ar;'_x_doc  =   Uirv/_DIS_pdu->entity_velocity  .X   •    FT_PER_METERS; 

Au"v'_>_doc  =   UUV_DIS_pdu->entity_velocity  .y    •    FT_PER_METERS; 

A'-"v_r_doc  =   UUV_DI£_pdu->entity_velocity  .  z    •    FT_PER_METERS; 


0] 
1] 

2] 


Airv'_phi_doc      =radians  (UUV_DIS_pdu->dead_reckon_parains  .angular_velocity 

A'.^_:  ;;tr:i_doc  =  radians  ;UlW_DI£_pdu->dead_reckon_paranis  .  angular_velocity 

A"JV_psi_dot      =radian£  (UW_DI£_pdu->dead_reckon_parains  .  angular_velocicy 

cout  <--■  "viewer:  World  coordinace  velocities:    "  ; 

couc  <-^  "  1  "  ; 

couc  «-          UU\'_DI£_pdu->enciCy_velocity  .X                      <<    " , 

cout  <--'          L"UV_DI£_pdu->entity_velocity  .y                      <<    ", 

cout  «          UU"\/_DI£_pdu->entity_velocity .  z                     <<    ", 

couc  <-           inj\'_DI£_pdu-^dead_reckon_params .  angular_velocity    [0]    <<    ",    "; 

cout  «          UU\'_DIS_pdu->dead_reckon_parains.  angular_velocity    [1]    <<    ",    ■  ; 

cout  «          UUV_DIS_pdu->dead_reckon_paraiTis.  angular_velocity    [2]    <<    "]" 

<•'■  end!  ; 

cout  <<  endl; 

//  Note  that  even  though  the  accelerations  are  calculated  in  the  superclass 
//  UUVBody,  use  of  global  state  vector  lets  us  construct  a  class  hierarchy 
//  based  on  problem  structure  instead  of  the  coiranuni cat  ions  dependencies. 
//  This  is  proposed  as  a  general  DlS-compatible  vehicle  object  hierarchy. 

//  Accelerations  are  not  produced  in  world  coordinates,  thus  zeroes  expected 

AUV_u_dot  =  UUV_DIS_pdu->dead_reckon_params . linear_accel  (0)  *  FT_PER_><ETERS 

AUV_v_dot  =  UUV_DIS_pdu->dead_reckon_parains .  linear_accel  (1)  *  FT_PER_>dETERS 

AUV_w_dot  =  UUV_DIS_pdu->dead_reckon_parains . linear_accel  [2]  •  FT_PER_HETERS 


cout  « 

cout  « 

cout  « 

cout  « 

couc  « 


'viewer:  World  coordinate  accelerations:  "; 

■  1"; 

UUV_DIS_pdu->dead_reckon_parains.  linear_accel  (0)  «    •,    •; 

UUV_DIS_pdu->dead_reckon_parains.  linear_accel  [1]  «    •,    •; 

UUV_DlS_pdu->dead_reckon_parains.  linear_accel  [2]  «   ■]•   «  endl; 


//   what  we   look   like 

//   UUV_D1 £_pdu->enti ty_appea ranee; 

//   UUV_DI£_pdu->entity_rT)arking.  character_set; 
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/  /  UU\'_DIS_pd'j->entity_marking  .markings; 

//  project  our  movement 

UUV_DIS_pdu->dead_reckon_params .algorithm  =  DRAlgo_DRM_FPW; 


//  cout  <<  "viewer:  DIS_net_read 
//  cout  <<  flush; 


successful"  <<  endl ; 


//  update  overall  AUV  posture  (both  position  &  orientation) 
A'J\'_DGSition_node->translat  ion.  setValue  (  AUV_x,    -  AUV__y, 


AUV_z ) ; 


rotace_AUV_z-> 
rotate_AUV_y- > 
rotate  AUV  x-> 


angle. setVaiue  ( 
angle. setValue  ( 
angle. setValue  ( 


AUV_psi) ; 
AUV_theta) ; 
AUV_phi  )  ; 


Art  iculatParamsNode    •    APNptr   =   UUV_DIS_pdu->articulat_paranis_head; 


AUV_cime  = 

AUV_time  *= 

AW_dfrlta_rudder   = 
AlV_del ta_planes   = 


APNptr->articulat_params .parameter_value  10] ; 

APNptr->articulat_params .parameter_value  [1]  /  10.0; 

APNptr->articulat_params .parameter_value  [2] ; 

APNptr->articulat_params .parameter_value  [3] ; 


/'    denorTTiaZ  J  Z'^    fcrmer   short?   to   be  negative   if   necessary 
if     iA'J\'_deIta_rudder    >=    128.0)    AU\'_dei ta_rudder   -=   256.0; 
if     (A'JV_de:ta_planes    >=    126.0)    A"J\_del  ta_planes    -=   256.0; 


A'JV_port_rpm 
if   ,AUV_port_rpm 
Al'\'_p.ort_rpiTi 
if   iA'J'v'_pGrt_rpm 
Al.V_port_rpm 
el^e   A'j'J_po r t _rprri 


=  APNptr->articulat_params.parameter_value  [4 

>=  128.0)  Air.'_port_rpm  =  AU\'_port_rpm  -  256.0; 

•=  10.0; 

>=  0.0) 

■f=  APNptr- >art iculat_params .parameter_value  [5 

-=  APNptr->articulat_params.parameter_value  [5; 


AU"v'_stbd_rprT  =  APNptr->articulat_params  .parameter_value  [6]; 

if   {AUV_stbd_rpm  >=  128.0)  AUV_stbd_rpm  =  AUV_stbd_rpm  -  256.0; 

ALV_3 1  bd_rpm  *  =  10.0; 

if   (Airv'_stbd_rpm  >=  0.0) 

A'JV_stbd_rpm  +=  APNptr->articulat_params  .parameter_value  [7]; 

else  ALr.'_3tbd_rcn-  -=  APNptr->articulat_params  .paramieter_value  [7]; 

//  ccut  <<    "viewer:  Articulation  parameter  0:"  <<  endl; 

c'ut   -'-'    "AU";'_Lime  =    "    <<  AU\'_tiirie  «   endl 


cout 


:<-  "A'J\'  delta  rudder  = 


//  ccut  <r< 
II  cout  << 
II   cout  ^<  ■AUV_3tbd_rpn"i 


■ALn;_delta_planes 
'A'JV_port_rpir. 


"  <<  AUV_del ta_rudder  <<  endl 

"  «   AUV_del ta_planes  «  endl 

"  <<  AUV_port_rpmi     «  endl 

"  <<  AUV_stbd_rpm    <<  endl; 


APNptr  =  APNptr->next_articulat_params;  //  next  articulated  parameter 


//  thrusters 
A'.A,'_bow_ver  1 1  ca  1 
AUV_s t e rn_ver t i ca 1 
AUV_bow_lateral 
AUV  stern  lateral 


=  APNptr->articulat_params .parameter_value  [0] ; 

=  APNptr->articulat_params .parameter_value  [1]; 

=  APNptr->articulat_params .parameter_value  [2]; 

=  APNptr->articulat_params .parameter_value  [3]; 


//  denormalize  fonner  shorts  to  be  negative  if  necessary 

if  (AUV_bow_vertical   >=  128.0)  AUV_bow_vertical    -=  256.0 

if  (AUV_stern_vertical  >=  128.0)  AUV_stern_vertical  -=  256.0 

if  (AUV_bow_lateral    >=  128.0)  AUV_bow_lateral    -=  256.0 

if  (AUV_stern_lateral   >=  128.0)  AUV_stern_lateral   -=  256.0 

//  convert  thruster  volts  to  force  by  signed  squares  &  normalize  adjust 


AUV_bow_vertical  =  AUV_bow_vertical 

AUV_stern_vertical  =  AUV_stern_vertical 

AUV_bow_lateral  =  AUV_bow_lateral 

AUV_stern_lateral  =  AUV_stern_lateral 

//  slots  4.. 7  as  yet  unused 


fabs (AUV_bow_vertical   )  /  24.0 

fabs(AUV_stern_vertical)  /  24.0 

fabs(AUV_bow_lateral   )  /  24.0 

fabs(AUV_stern_lateral  )  /  24.0 
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ccut  << 


I  I 
I  ! 

I  ■     COUt 

//  COUt 

//  COUt 

COUt 


"viewer:  Articulation  parameter  1: 
endl  ; 

"AUV_bow_vertical  = 
•AUV_stern_vertacal  = 
"AUV'_bow_lateral  = 
■AiJ\'_3ten-._Iateral   = 


thrusters" 


«  AUV_bow_vertical  «  endl 

«  AUV_stern_vertical  «  endl 

«   AUV_bow_lateral  «  endl 

<<  AUV  stern  lateral  «  endl 


APNptr  =  APNptr->next_articulat_parains;  //  next  articulated  parameter 


AlTV_ST1000_bearing  = 

A'JV_£'T1  000_range 
AUV_ST1000_strength  = 
A  W_ST7  2  E;_bea  r  ing 

AUV_£T7  25_ranqe 
AlT\'_ST7  2  5_strenqth  = 


APNptr ->articulat_params .parameter_value  10) 

APNptr- >articulat_params.parameter_value  [1 ] 

APNptr->articulat_params .parameter_value  [2 1 

APNptr->articulat_parajns.parameter_value  [3  ] 

APNptr->articulat_params .parameter_value  [4 ] 

APNptr->articulat_params.parameter_value  [5 ] 

APNptr->articulat_paroLms.parameter_value  [6  ] 

APNpt r->articulat_params.parameter_value  [7 ] 


•  10  + 
/  4.0; 

•  10  + 
/  4.0; 


//  COUt  <<  "viewer:  Articulation  parameter  2:   sonar"  «  endl; 


//  COUt  <<  •AUV_ST100  0_bearing 

//  COUt  <<  •AUV_ST1000_range 

//  COUt  <<  •AUV_£T1000_strength 

//  COUt  «-  "AUV_ST725_bearing 

//  COUt  <<  "AUV_ST7  2  5_range 

//  COUt  <<  "AUV_ST725_strength 


«  AUV_ST1000_bearing  «  endl 
«  AUV_ST1000_range  «  endl 
«  AUV_ST1000_strength  «  endl 
«  AUV_ST72  5_bearing  «  endl 
«  AUV_ST72  5_range  «  endl 
<<  AUV_ST725_strength  «  endl 


//  Frint  hostname  of  PDU  (revision  in  network .round  version) 

'.■  hp  =  gethostbyaddr  (  (char  •)  iinaddr,  sizeof  ( struct  in_addr),  AF_INET) 

//  COUt  <-<  "viewer:  Host  name:   "  <<  hp->h_name  <<  endl; 

//  don't  forget  freePDU  or  get  a  memory  leak! 

•/  articulated  parameters  are  also  freed 


ireePZ'U  (local  PDL'i 


/ /  ecu: 


•viewer:  freePDU  ilocai_PDU)  called  for  this  PDU"  «  endl; 


,'  en:j  wn.^e  intinite  lOOp 

COUt  ••  "viewer:  L)I£  net_read  portion  complete,  now  update  scene  graph. 
<■-  endl; 


del 

/  / 
// 
// 
// 

if 
{ 


ierit_cic;k  =    clock    (); 

ti_cloc<       =  current_clock  -  time_of_PDU_receipt ; 

:i_tinie        =  (double)  delta_clock  /  CLOCKS_PER_SEC; 


tout  ■-<  "viewer:  current_clock  =  " 

<-<  ",  time_stamp_of_current_PDU 
•'<  ",  time_of_PDU_receipt  =  " 
«  ",  PDU  delta  time  =  " 


«  current_clock 
«   time_stajnp_of_current_PDU 
<<  time_of_PDU_receipt 
«  delta_time         «   endl; 


f (delta_time  >=  0.0)  &&  (delta_time  <=  5.0))  //  update  positions,  postures 


delta_x 
de  1 1  a_y 
delta_2 

del ta_phi 

delta_theta 

delta_psi 


=  AUV_x_dot 
=  AUV_y_dot 
=  AUV  z  dot 


*  delta_time; 

*  delta_time; 

*  delta_time; 


=  AUV_phi_dot  •  delta_time; 
=  AUV_theta_dot  *  delta_time; 
=  AUV_psi_dot   •  delta_time; 


//  COUt  «  "viewer:  DeadReckon_Callback :  • 

//  «  •   PDU  delta_time  =  "  <<  delta_time 

//  COUt  «  •  AUV_phi       =  •  «  degrees  (AUV_phi ) 

//  <•<  " ,    AUV_phi_dot          =    "    «  degrees    (AUV_phi_dot ) 

//  COUt  «  "   AUV_theta      =  "  «  degrees  (AUV_theta) 

//  «  ' ,    AUV_theta_dot   =  *  «  degrees  (AUV_theta_dot )  «  endl 


«  endl 
«  endl 
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.'/  couc  <<  "   delca_x  =  '  <<  delCa_x  <<  endl 

//  cour  <<  "   delta_y  =  "  <<  delta_y  <<  endl 

//  cout  <<  "   delta_z  =  "  <<  delta_z  <<  endl 
//  couc  <<  endl ; 

//  save  current  position  for  next  time  the  camera  is  repositioned 
AU\'_x_prior  =  AUV_x; 
AUV^-_prior  =  AlTV_y; 
AUV_z_prior  =  AU\'_z; 

//  graphics  rendering  problems-,   psi ,  rudders  are  opposite 

//  update  overall  AUV  posture  (both  position  &  orientation) 

AUV_posi tion_node->translation .setValue  (  AUV_x     +  delta_x, 

-  (AU\'_y     +  deltas/)  , 

-  (AUV_z     +  delta_z)); 
rotace_AUV_z->anqle. setValue         (  -  (AUV_psi    +  delta_psi ) ) ; 
rotate_AlA'_y->angle. setValue         (  -  (AUV_theta  +  delta_theta) ) ; 
rot3t€_AUV_x->angie . setValue            (AUV_phi    +  delta_phi); 

//  update  AU\'  rudder  i  plane  orientations 

rota" t^_AlA,'_bow_rjdders--^angle.  setValue  (  radians  {AU\'_delta_rudder)  ) 

rotate_AUV_steni_rudders->angle. setValue  (  -  radians {AUV_delta_rudder ) ) 

rctate_AW_bow_j:'lanes->anQie  .  setValue  (  -  radians  (AUV_del  ta_planes  )  ) 

rct3tfc_Al"v/_stern_ciane3->angle  .  setValue  (  radians  (Airv'_delta_planes)  ) 

//  rout  <<  •AlT\'_delta_rudder  =  "  <<  AUV_delta_rudder 

/  /  «  ",  radians  (AUV_del ta_rudder i  =  * 

//  -'<■  radians  (AUV_delta_rudder ) 

-:^  endl  ; 

//  cout  <-:  "AU'v'_del  ta_planes=  "  «  AUV_del  ta_planes 

//  <'■  ",  radians  (A'J'v'_delta_planeS)  =  " 

/'  <<  radians  f  AUv'_de  It  a_p  lanes  ) 

/ /  «  endl ; 

if  ; fabs (AUV_stbd_rpm  - 

(conePropellerWakeStbd->height .getValue{ )  *  700.0/24.0))  >  10.0) 
//  ensure  needed 
( 

conePropellerWa)ce£tbd->height       =       AUV_stbd_rpm  /700. 0*24.0; 

cone?ropelIerWakeStbd->bottoinRadius  -    fabs  (AUV_stbd_rpm)  /700.0*  6.0, 

if  !fab=  ;A'J\'_port_rpiTi  - 

(conePropellerWa)cePort->height  .getValue( )  •  700.0  /  24.0))  >  10. 0; 

//  ensure  needed 

(' 

conePropellerWa)<ePort->height       =       AUV_port_rpm  /700. 0*24.0, 
conePropellerWa)^ePcrt->bottomRadius  =  fabs  (AUV_port_rpm)  /700.0*  6.0; 

) 

if  ( fabs (AUV_bow_vertical- (thrusterWakeFV->height .getValue ( ) /2 .0) )  >  l.O; 

/'  ensure  needed 

thrusterWakeFV->height  =  -  AUV_bow_vertical  *  2.0; 
thrusterWakeFV->bottomRadius  =  AUV_bow_vertical  /  4.0; 
if    (AUV_bow_vertical  <  0.0) 

//  bottomsideBow,  negative  volts  push  up   (negative  direction) 

whichWakeFV->whichChild  =  1; 

//    topsideBow,  positive  volts  push  down  (positive  direction) 
else  whichWakeFV->whichChild  =  0; 

topsideBow->   translation. setValue (0,    TOPHALF+AUV_bow_vertical , 0) , 
bottomsideBow->translation.  setValue  (0,  BO'ITCMHALF+AUV_bow_vertical ,  0)  , 

} 

if  (fabs (AUV_stern_vertical- (thruscerWakeAV->height .getValue ( ) /2 .0) ) 
>  1.0) 

//  ensure  needed 
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t:hru3terWakeAV->height  =  -  AUV_stern_vertical  *  2.0; 
thruscerWakeAV->bottomRadius  =  AUV_stern_vertical  /  4.0; 
if    (AlJV_scern_vertical  <  0.0) 

//  boccomsideStern,  negative  volts  push  up   (negative  direction) 

whichWakeAV->whichChild  =  1; 

//    topsideStern,  positive  volts  push  down (positive  direction) 
else  whichWakeAV- >whichChild  =  0; 

top.^:  JeStern-  >        t  ranslat  ion  .  setValue  (0  ,    TOPHALF+AUV_stern_vertical ,  0) 
bct!:om3ideEtern---translation.setValuefO,BOTTOMHALF+AUV_stern_vertical,0) 
) 

if  (fabj  ■AL"v'_bow_lateral- (thrusterWakeFH->height  .getValueO  /2.0))  >  1.0) 
//  ensure  needed 
i 

t.hrjsterWakeFH- -.-height  =  -  Airw'_bow_lateral  ♦  2.0; 
thrusterWakeFH->bottomRadius  =  AUV_bow_lateral  /  4.0; 
1  f    ( AiJw'_bow_l a t era  1  <  0.0) 

//  right-ideEow,  negative  volts  push  left   (negative  direction) 
whichWakeFH->whichChi Id  =  1; 

//  leftsjdeBow,  positive  volts  push  right  (positive  direction) 
else  wnichWakeFH->whichChi Id  =  0; 

lef tsideEow->  translation . setValue  (  0,  LEFT_HALF+AUV_bow_lateral ,  0); 

rig.'-it£ideBow->translation.  setValue  (  0,  RIGHTHALF+AUV_bow_lateral ,  0); 
i 
if  (fabs  (Airv'_stern_lateral-  ( thrust erWakeAH->height  .getValue  ( )  /2.0)  ) 

>  1.0) 
.' ;  e.nsure  needed 

thrusterWakeAH->height  =  -  AU\'_stern_lateral  *  2.0; 
trir  j£terWakeAK->bot  tomF.adius  =  AUV_stern_lateral  /  4.0; 
if    (AlTV'_stern_lateral  <   0.0) 

//  rightsideStern,  negative  volts  push  left  (negative  direction) 

wr.ichWakeAH->whichCr.:  id  =  1; 

/ /'   lef  tsideStern,  positive  volts  push  right  (positive  direction) 
else  whichWakeA.H->whichChild  =  0; 

ief  t  Sides  tern- ■>  translation  .  setValue '0,  LEFT_HALF+AUV_stern_lateral ,  0)  ; 
1  .gntsi  je£:tei-;,-^ti-anslatior, .  setValue  i  0,  R I GHTHALF-^Al^v'_stern_l a teral ,  0)  ; 

::   ::-i;:.^  .A'.'"._£T:CM:_range  -  coMe£cnar£T1000->height .  getValue  ()  )  >  0.0) 
/  /  er.sure  needed 

.:-ne£onar£T:noo->height       =  fabs  (AUV_ST1000_range)  ; 
cone£onarST1000->bottoinRadiu?  =  fabs  (AUV_ST1000_range)  /  60.0; 

//  1  degree 
>:fCcne£;cnar£71000->translation.  setValue  (   0.0, 

-  (AUV_ST1000_range  /  2.0), 
4.0  /  12.0  ) ; 
i 
} 

else  if  (PDU_overdue  ==  FALSE)  //  update  scene  graph,  reset  vehicle  position 
{ 

PDU_overdue  =  TRUE;  //  over  5  seconds  elapsed  since  last  PDU 

//  restore  latest  valid  AUV  posture  (both  position  &  orientation) 

AUV_position_node->translation. setValue  (  AUV_x, 

-  AUV^, 

-  AUV_z )  ; 
rotate_AUV_2->angle. setValue           (  -  AUV_psi); 
rotate_AUV_y->angle. setValue           (  -  AUV_theta); 
rotate_AUV_x->angle. setValue           (  AUV_phi ) ; 

//  thrusters 

AUV_bow_vertical    =0.0; 
AUV  stern  vertical   =  0.0; 
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A'JV'_bo  w_  lateral 
A'JV  stern  lateral 


=  0.0; 
=  CO; 


r  i'l  i''j  i:  1 1? ;' Wa  k  e  Pv 
thrusterWakeFV 
thrusterWakfeAV 
thrusterWakeAV 
chrusterWakeFH 
thrusterWakeFH 
thrusterWakeAH 
thrusterWakeAH 


■  -height  =  0.0 

->bottomRadius  =0.0 

■>height  =  0.0 

■vbottomRadius  =0.0 

->height  =0.0 

->bottoniRadius  =  0.0 

■>height  =  0.0 

->bottomRadius  =  0.0 


AUV_ST1000_bearing  =  0 
AUV_ST1000_range  =  0 
AUV_ST100C_strencth  =  0 
AU\'_ST7  2  5_bearing  =  0 
AlA'_ST7  2  5_range  =  0 
AU\'_ST725_strength   =  0; 


coneSona  rSTl  00  0-  ^-hei  ght 
coneSonarST100  0->bottomRadius 


fabs (AUV_ST1000_range) ; 

fabs (AUV_ST100C_range) /60.0;  111   degree 


AUV_port_rpir, 
AUV_stbd_rpir, 


0.0; 
0.0; 


cor.ePropel  lerWakePort->he:ght  =0.0 
conePropei lerWakePort->bottomRadius  =  0.0 
conePropelierWakeStbd->height  =  0.0 
conePropellerWakeStbd->botcomRadius  =0.0 

ccut  <<    "viewer:  DeadReckon_Callback :  " 

<-•--  "Pr)U  delta_time  =  "  <<  delta_time  <<  ■ ,  " 

<<   endl ; 
couc  <--<  "viewer  posi tion/pcsture  reset  to  last  received  PDU.  "  «  endl; 
ccut  <<  end! ; 

ll     giODalS 

priorAUVPosi tion  =  SbVecSf  (AUV_x_P'rior,  AUV_y_prior,  AU\'_2_prior)  ; 
currentAUVPcsition  =  SbVec3f  (AUV_x,       AUV_y,       AUV_z); 
aheadOf AUVPosi tion  =  currentAUVPosition  -»■ 

SbVec3f  (10.0-cos  (AUV_psi),  10 .0*sin (AUV_psi ) , 2 . 0) ; 

switch  I  whichCamera  )  //  reposition  appropriate  camera  as  needed 

{ 

case  CAMERA_FREE: 

priorCameraPosi tion   =  PerspectiveCameraFree->position.getValue  (); 

priorCameraOf f set    =  priorCameraPosition  -  priorAUVPosition; 

C'jrrentCarTieraPosition=  priorCameraPosition; 

PerspectiveCameraFree->orientation.getValue (orientationRotationAxis, 

orientationRotationAngle) ; 

break ; 


case  CAMERA_TO_AUV :  //  retain  camera  pos'n  relative  to  new  AUV  posit 
priorCameraPosition  =  Per spec tiveCameraToAUV->posi tion. get Value 
priorCameraOf f set   =  priorCameraPosition  -  priorAUVPosition; 
//  verify  here 

currentCameraPosition  =  ( 

+ 
PerspectiveCameraToAUV->position.setValue     ( 

+ 

PerspectiveCameraToAUV->pointAt  (currentAUVPosition  ); 

PerspectiveCameraToAUV-> focal Distance. setValue (standardCameraFocalDist 
PerspectiveCameraToAUV->orientation.getValue   (orientationRotationAxis 

orientationRotationAngl 
break; 


currentAUVPosition 
standardCameraOf f set 
currentAUVPosition 
StandardCameraOf f set 


ion 
{); 


); 
); 

ance) ; 
4  ); 
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couc 

<< 

ccut 

<< 

<< 

<< 

<< 

« 

•* 

^ 

" 

<< 

<< 

" 

" 

<< 

<■' 

" 

" 

<< 

<< 

"  > 

M 

<< 

case  CAWERA_FROM_AUV:  //  retain  camera  position  looking  out  from  AUV  pos . 
priorCameraPosi tion  =  PerspectiveCameraFromAUV->position . getValue  (); 
priorCameraOf f set    =  priorCameraPosi tion  -  priorAUVPosition; 
verify  here 

c'_rrentCameraPosi  t  ion  =  (  currentAUVPosition  )  ; 

PerspectiveCameraFromAUV->position . setValue      (  currentAUVPosition  ) ; 
Perspect iveCameraFromAUV->pointAt  (  currentAUVPosition 

+standardCameraOf f set  ) ; 
Perspect :veCamer a FromAUV->f oca  1 Distance. setValue (standardCameraFocal Distance) ; 
Perspect iveCameraFromAU\'->orientation . getValue   (orientationRotationAxis, 

orientationRotationAngle  ) ; 
break; 

)  //  end  switch  (  whichCamera  ) 

//  print  out  all  camera  parameters 
endl  ; 

AUV_position     =  <"  «  (AUV_x) 
",.  •  «  (AUV_j/) 

",  "  «    {AUV_z)  «  •>  " 

endl  ; 
cout  <■-'  "delta_AUV_position    =  <-"  «  (AUV_x  -  AUV_x_prior) 
<<    " ,    "  «    (AUV_y  -  AUV_y_prior) 

<<  " ,  "  «  (AUV_z  -  AUV_z_prior)   «  •>  ■ 

<<   endl ; 
cout  <<  "del ta_CameraPosi tion  =" 

icurrentCameraPosition  [0]  -  priorCameraPosition  [0]) 
(currentCameraPosition  [1]  -  priorCameraPosition  [1]) 
(currentCameraPosition  [2]  -  priorCameraPosition  [2]) 
endl  ; 

cout  <<        "priorCameraPosition   =" 
<v  "  <"  .'V-  priorCameraPosition  [0] 
<-'  "  ,  "  <^  priorCameraPosition  [1] 

<•.  ",  "  <<  priorCameraPosition  [2]  <<  ">"  <<  endl  ; 
couc  <<        "priorCameraOf f set     =" 
<<  "  <"  <<   priorCameraOf f set  |0] 
<<  " ,  "  <<   priorCameraOf f set  [1] 

<-.'",  "  <■'  priorCameraOf  f  set  12]  <<  ">"  <<  endl  ; 
cout  <<  "currentCameraPosition  =" 

■:<■    "  <-"  <•'-  currentCameraPosition  [0] 
•r-  ",  •'.'•'  currentCameraPosition  [1] 
<<  " ,  "  <<   currentCameraPosition  [2]  <<  ">"  <<  endl ; 
.-Que  ••-•        "oriencationRotat ion   =" 

',-.-  "  ••"  -;--.  orientationRotationAxis  [0] 
c<-  ",  -  <<  orientationRotationAxis  |1] 
■'<■",  "  --••  orientationRotationAxis  [2] 
<<  " ,  "  <<  degrees  (orientationRotationAngle)    «  '>"  <<  endl ; 

/  /  cout  •'<  "viewer:   end  of  DI£_Redraw_Callback  ()"  <<  endl  ; 

return; 

) 

// // 

//  called  on  an  exit  condition  via  a  call  to  atexit  (DI£_net_close)  in  main 

static  void  DIS_net_close  () 
( 

cout  <<   "viewer:  DIS_net_close  ();"  <<  endl; 

net_close  ( ) ; 

DIS_port_open  =  FALSE; 

} 

// // 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 II II 1 1 1 1 1 1 II 1 1 1 1 II 1 1 II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 II 1 1 1 1 1 1 1 1 1 1 II 1 1 1 
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//   This  is  called  by  the  Color  Editor  whenever  the  color 
//   has  changed.  The  userData  is  set  by  main()  in  the  call 
to  ScXtColorEdi tor : :addColorChangedCallback . 

vo^d 

colcrEditcrCB I  void  *userData,  const  SbColor  *rgbCallbackData  ) 

{ 

SoXtRenderArea  *renderArea  =  (SoXtRenderArea  *)  userData; 

rendfrrArea-->setBackgroundColor  (  *rgbCallbackData  )  ; 
) 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

ScSeparator  *  readFile (const  char  'filename)  //  Inventor  Mentor  p.  284 
I 

//  Open  the  input  file 

Solnput  myScenelnput ; 

if  ■ ! myScenelnput . open File (filename) ) 

r::^x.    ■  ■'-    "Cannot  op-en  file  "  <<  filename  «   endl  ; 
return  NULL; 

/ /  Read  the  whole  file  into  the  database 

ScSeparator  *  myGraph  =  SoDB : : readAl 1 ( tmyScenelnput ) ; 

.f  .rryGraph  ==  NULL; 

cout  <<  "Problem  reading  file  "  <<  filename  <<  endl ; 

return  NULL; 
) 

m^'Scenelnput  .closeFile    (); 
return  myGraph; 

!!  I  f  I  !■  I  '.'■''!!  n  1 1 1 1 1 1  '  1 1 1 1 1 1 1 1 1 1  n  1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1  / 1 1 11 1 1 


void  in:  t  iali  2e_globals  ■,  ) 

/,'  mult. cast  pert  &  group 
strncpy  (port,   "3111",    4); 
strncpy  i  group,  "224.2.121.93",  12); 

//  3111  is  npsnet  'standard'  port 

//  #define  DEFAULT_PORT  =  "3111"; 

//  #define  DEFAULT_GROUP  =  "224.2.121.93"; 

//  initialize  materials 

silver->  amtientColor . setValue  (  .2,  .2,  .2  ) 
siiver->  diffuseColor. setValue  (  .5,  .6,  .6  ) 
silver->specularColor . setValue  (  .5,  .5,  .5  ) 
s: lver->shininess  =  .5; 

gold-^  ambientColor. setValue  (  .4,  .2,  .0  ) 

gold->  diffuseColor . setValue  {  .9,  .5,  .0  ) 

gold->specularColor . setValue  (  .7,  .7,  .0  ) 
gold->shininess  =  .6; 

Lrass->  ambientColor . setValue  (  0.329412,  0.223529,  0.027451  ) 

brass->  diffuseColor .setValue  (  0.780392,  0.568627,  0.113725  ) 

brass->specularColor .setValue  (  0.992157,  0.941176,  0.807843  ) 
brass->shininess  =  0.21794872; 

chrome->  ambientColor . setValue  (  0.25,  0.25,  0.25  ); 

chrome->  diffuseColor . setValue  (  0.4,   0.4,   0.4  }; 

chrome->specularColor .setValue  (  0.774597,  0.774597,  0.774597  ) 
chrome->shininess  =  0.6; 
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npsblue->  ambiencColor . seCValue  (  0.0,  0.0,  l.C  )  ; 

npsLlue->  di f f JseCoior . setValue  i  0.0,  O.C,  0.8  ) ; 

npsblue->specularColor . setValue  (  0.0,  0.2,  1.0  )  ; 
npsblue- >shininess  =  0.8; 

seagieen->  ambientColor . setValue  (  0.0,  0.5,  0.0  ) 

■3eagi-<den->   diff  useCoior .  setValue  {  CO,  0.5,  0.0  ) 

seac!reen->specularColor.  setValue  (  0.0,  0.5,  0.0  ) 
3eagreer;->shininess  =  0.0; 


darkareen-->  ambientColor .  setValue  (  0.15,  0.20,  0.15  ) 

daiKui-er,-'  di  ffuseCclor .  setValue  i  0.15,  0.20,  0.15  ) 

darkgreen- ^specularColor . setValue  (  0.15,  0.20,  0.15  ) 
d":i  rkcreen-  -sni  n:  nes?  =  0.0  ; 


sonai_red->  ambientColor . setValue  :  1.0,  0.0,  0.15  ) 

scnar_red-^  di ffuseColor . setValue  (  1.0,  0.0,  0.15  ) 

sonai_red->3pecularCGlor . setValue  (  1.0,  0.0,  0.15  ) 
sonar_red-  -shininess  =  G.O; 


return ; 
I   .' ,'  end  initialize_giobals  () 

,:! i :■''  1 1  i !■:  I  I :  I ;  1 1 1 1 1 1 !  1 1 1 1 1 1 !:! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 1 1 1 
■  1 1 1 1 1 1 1 !  1 1 1 1 1 ;  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

veld  t'3rse_comi:iand_line_f lags  (int  argc,  char  **  argv) 

//    comrr.and  line   arauments 


:nt    index,    i 
/  /■    c  ou  t    --<    " 


■  ;    icur  •  ■- 
//  foi  ;i  = 
/'  cout  <  <- 


"  iparse_comiTiand_line_f lags  start:  #  arguments 
endl  ; 


<<  argc  <<  "] 


0;  1    <   argc;  i^ 
'  ] "  <<   end- ; 


I  cout  <<  argv  [ij  << 


-  1 


arac;  i- 


for  lindex  =  0;  index  <=  strlen  (argvii]);  index++; 
argvfij  [index]  -  toupper  (argv[i)  [index]); 

::"      ''strcTip  fargvii],  "PORT")  ==0!  il 

(strcmp  !argv[i],  "-PORT")  ==  Oi  li 

(strcmp  iargvli],  "P")  -=   0)  II 

'strcmp  'argv;i],  "-P")  ==0)) 


//  uppercase 


if 

e.1  St 
( 


} 


I  i-'i  >=  argc  ) 
cout  •----  "Insufficient  parameters  for  PORT"  «  endl; 


cout  «  "["  <<  argv[i]  «  "  "  <<  argv[i+l]  «  "]"  <<  endl; 
strcpy  (port,  argv[i+l])  ; 
i++  ; 


] 

else  if  ((strcmp  (argv[i] 

(strcmp  (argv[i] 

(strcmp  (argvii] 

(strcmp  (argvii] 

(strcmp  (argv[i] 

(strcmp  (argvii] 

(strcmp  (argv[i] 

(strcmp  (argvii] 


■GROUP" )  ==  0) 

■-GROUP" )  ==  0) 

■G")  ==  0) 

■-G")  =-  0) 

■ADDRESS")  ==  0) 

"-ADDRESS")  ==  0) 

"A")  ==  0) 

■-A")  ==  0) 


if   (  i+1  >=  argc  ) 

cout  «  ■Insufficient  parameters  for  GROUP  ADDRESS"  «  endl; 
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ei  se 


COU1   <'-  "I"  <<  argviij  << 
scrcpy  (group,  argv(i+lj); 

cout  -v-  "i"  <<-  argv[i]  << 


<<  argvli+1]  <<  ")"  <<  endl 
<<   argv[i+l]  «  "]"  <<  endl 


else 
{ 


} 

elst 


if  ( (strcmp  (argviij,   " PRINTDIALOG" )  ==  0) 
(strcmp  (argviij,  " - PRINTDIALOG" )  ==  0)) 

PRINTDIALOG  =  TRUE; 

cout  <<  "["  <<  argv[ij   «  "j"  <<  endl ; 


if  (  (strcmp  (argv [ i  j 

(strcmp  (argv[i] 

(strcmp  {argviij 

(strcmp  (argviij 

(strcmp  (argviij 

(strcmp  (argvjij 


"TEXTURE")  ==  0) 

-TEXTURE")  ==  0) 

"TEXTURE-ON";  ==  0) 

-TEXTURE-ON")  ==  0) 

"T"  )  ==  0) 

-T"  )  =-  0) ) 


TEXTURE  =  TRUE; 

cout  <<  "("  «   argvli]   «  "J"  <<  endl; 


e,  se  ^  t  I 


'strcmp  (argviij 

t strcmp  (argviij 

(strcmp  'argviij 

(strcmp  ; argviij 


"NOTEXTURE" ;  ==  0 )  I  I 

'-NOTEXTURE" )  ==0)  II 

"TEXTURE- OFF" ;  ==  0 )  II 

'-TEXTURE-OFF" )  ==  0) ) 


"EXTURE  =  FALSE; 

rout  <  <•  "  i  "  <•'  argv  I  i  ] 


]  "  <<  endl ; 


eise 


If  '(Strcmp  (argviij,  "FILE")  ==0)  II 

'strcmp  largviij,  "-FILE")  ==0)  II 

(strcmp  largvdj,  "F"'  ==0)  II 

;  strcmp  largvtij,  "-F")  ==  Oi) 


if 


eise 


,  1*1  "'-=  argc  ; 

cout  <•<  "Insufficient  paramieters  for  FILE  filename,  iv 
<<  endl ; 


cout  <<  "i"  <<  argviij  <<  "  "  <<  argvli+l]  «  "]"  <<  endl 
command_line_node  =  new  SoSeparator; 
command_line_node  =  readFile  (argvii+lj); 
root->    addChild  (  command_line_node  ) ; 
i  +  + ; 


else 


else 
( 

} 
else 

{ 


if  ((strcmp  (argviij,   "NOPRINTDIALOG" )  ==0)  II 
'Strcmp  (argviij,  "-NOPRINTDIALOG")  ==0)) 

PRINTDIALOG  =  FALSE; 

cout  <<•  "1"  <<   argviij   <<  "]"  <<  endl; 


if  ( (strcmp  (argv I i 
(Strcmp  (argvli 


"BACKGROUNDCOLORDIALOG" )  ==  0) 
'-BACKGROUNDCOLORDIALOG" )  ==  0)) 


BACKGROUNDCOLORDIALOG  =  TRUE; 

cout  <<  "I"  «  argvli]   <<  "]"  <<  endl; 

if  ((strcmp  (argv[i],   "NOBACKGROUNDCOLORDIALOG" )  ==0)  II 
(strcmp  (argvli],  "-NOBACKGROUNDCOLORDIALOG")  ==  0)) 

BACKGROUNDCOLORDIALOG  =  FALSE; 

cout  <<  "I"  «  argvli]   <<  "j"  <<  endl; 
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else  cout  <<  "Unrecognized  command  line  parameter:   '"  «  argv[i] 
<<    "',  ignored."  <•<-  endl; 

}   /  /  end  for  loop  through  command  line  parameters 

//  cour  '<"  '  [parse_command_l  ine_f  lags  complete)"  <<  endl  ; 

return; 

1    /  /    end  pdrse_comir(and_line_f  lags    () 

1 1 1 1 1 !,  1 1 1 1 1 1 1 1 1 1  .n  1 1 1 1 1  i  n  1 1 11 1  // 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 11 1  / 1 1 1 1 1 1 1 1 1 1 1/ 1 1 1 1 1 1 

:    ■ , . !  I  i .  i  1 1 1  r  i  I , ! 1 1  i  1 1! I ! I ;  1 1 1 1 1 ; 1 1 1  i  1 1 1 1 1 1 1 1  n  / 1 1 1 1 1 1 1 1 1 1 /  n  1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 

mam  (  int  argc,  char  '*  argv  ) 

//  Initialize  Inventor  and  Xt  -  these  steps  MUST  be  first  calls 
//  in  main,  without  exception,  or  a  mystery  crash  results. 
Widget  ViewerWindowWidget  =  SoXt : : init (argv ( 0) ) ; 
if  1  ViewerWindowWidget  ==  NULL  ) 

I 

cout  ••*-  "viewer:   ViewerWindowWidget  ==  NULL  on  startup,  exiting." 

«  endl; 
exit;  1  ) ; 
I 

cout  <-;  "viewer:   ViewerWindowWidget  added"  «  endl; 

init lali 2e_globals  '); 

parse_commar,d_line_f  lags  (argc,  argv); 

//  porr  and  croup  can  change  by  command  line  switches 
cout  -■  "m,:itic33t  port  =  "   <<  pert 

^-  ",  address  group  =  "  <<  group  <<  endl; 

cnur  -  ■-  "creating  the  scene  graph:"  «  endl; 

root  =  new  E'oSeparat  or ; 

root- .'lef  i  ,  ; 

cout  <<"  "root  added"  <-<   endl; 


■'/  corr<-ct  for  different  coordinate  system  -  not  yet  working 

/  '  EoFotsticr.XYZ   ■*  coordmateSystemFl  ip  =  new  SoRotationXYZ ; 

//  coordinateSystemFlip->angle . setValue  ;  M_PI  ); 

//  coordinateSystemFiip->  axis . setValue  (  SoRotationXYZ : :X  ); 

/  ■'  root- >addChi  Id  (  coordinateSystemFlip  ); 

SoPctati onXYZ  *roOx  =  new  SoRotationXYZ; 

roOx---angle.  setValue  (  0.0  ); 

roOx-  -  axis. setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro90x  =  new  SoRotationXYZ; 
ro90x->angle. setValue  (M_PI  /  2.0); 
ro90x->  axis. setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro270x  =  new  SoRotationXYZ; 
ro270x->angle. setValue  (3.0  *  M_PI  /  2.0); 
ro270x->  axis. setValue  (SoRotationXYZ: :X) ; 

PerspectiveCameraFree  =  new  SoPerspectiveCamera 
PerspectiveCameraToAUV  =  new  SoPerspectiveCamera 
PerspectiveCameraFromAUV  =  new  SoPerspectiveCamera 

//  can't  put  a  group  or  separator  above  camera 

//  cameras  using  pointAt  are  90  degrees  twisted  askew 

//  Create  a  camera  SoSwitch  node 
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SoSw:tch  *  whichCameraSwitch  =  new  SoSwitch; 

roci:  -  -addChild  i  whichCameraSwitch  ;  ; 

whichCameraSwi  rch-->addChild  (  PerspecniveCameraFree    ) 

whichCameraSwi Lch->addChild  (  PerspectiveCameraToAUV    ) 

whichCameraSwi tch->addChi Id  (  PerspecciveCameraFromAUV  ) 

1 1    defaiiit    wi-.ichCaiTiera    defined   ac    top 

whjchCaiTieraSwi  tch--'WnichChild    =    whichCaniera  ; 

//  Create  a  camera  rotation  correction  SoSwitch  node 
SoSwitch  *  whichCameraCorrectionSwitch  =  new  SoSwitch; 
root-->addChild  (  whichCameraCorrectionSwitch  )  ; 
whichCamieraCorrectionSwitch->addChild  (  roOx   )  ; 
whichCameraCorrectionSwitch->addChiId  (  ro270x  ) ; 
whichCameraCorrectionSwitch->addChiId  (  ro270x  ) ; 
whichCameraCorrect ionSwi tch->whichChiId  -   whichCamera; 

root->addChild  (  new  SoPointLight  ) ; 

ScTransfcrm  '  xfLight2  =  new  SoTransform; 

xfLignt2- -translation. setValue  (0  .0,  0.0,  -  30.0); 

root-  -addCh: Id  (  xfLight2  ); 

root- ^addcmid  I  new  SoPointLight  ); 

SoTransform  '  xfLightS  =  new  SoTransf oitt:; 

xfLight3->translation.setVaIue (0 .0,  0.0,    30.0); 

root--addChild i  xfLight3  ); 

cout  •'•-  "2  point  lights  added  "  <<  endl ; 

SoXtP.enderArea    •  myRenderArea  =  new  SoXtRenderArea  (ViewerWindowWidget )  ; 
£r.\',ewc--rtPeg^  en   myPegion  imyRenderArea->getSi  ze  (j); 

currer.tA'A'Position  =  SbVec3f  iAL^'_x,  AW_y ,  AUV_z);     //  globals 
aneaoOf  A'J'yposi  t  ion  =  currentAUVPosition  -^ 

SbVec3f  (10.0  *  cos  (AUV_psi),  10.0  *  sin  (AUV_psi),  0.0); 

//  global 

//  Free  lunmiodi  f  ied  i  Camera 

Perspect iveCameraFree->viewAl 1  (root,  myRegion,  1.0);   //  global 

Perspe-tiveCameraFree->aspectRatio.setValue  (SO_ASPECT_VIDEO) ; 

/,'  >7amera  that  iceeps  h\N   in  center 

Per3peotiveCamera7oAUV-->viewAll  (root,  myRegion,  1.0);   //  global 
PerspectiveCameraToAU'v->aspectRatio.  setValue  (SO_ASPECT_VIDEOy  ; 
Perspect  iveCameraToAUV->posi cion . setValue 

(  currentAUVPosition  +  standardCameraOf f set  ) ; 
//    Perspect iveCameraToAUV->orientat ion. setValue 

(SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2.0  )); 
PerspectiveCameraToAUV->pointAt  (  currentAUVPosition  ) ; 

//  Camera  that  looks  out  fro.m  AUV  in  center 

PerspectiveCameraFromAUV->viewAll  (root,  myRegion,  1.0);   //  global 
Perspect iveCameraFromAUV->aspectRatio. setValue  (SO_ASPECT_VIDEO) ; 
PerspectiveCameraFromAUV->position .setValue  (  currentAUVPosition  ); 
//   Perspect iveCameraFromAUV->orient at  ion. setValue 

(SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2.0  )); 
PerspectiveCameraFromiAUV->pointAt  (  currentAUVPosition  +  standardCameraOf f set  ) 

unitsfeet  -   new  SoUnits; 

unitsfeet->units .setValue  (  SoUnits :: FEET  ); 

root->addChild (  unitsfeet  ); 

SoPicltStyle  *  unpickablestylenode; 
SoPickStyle  '  pickablestylenode; 
unpickablestylenode  =  new  SoPickStyle; 

pickablestylenode  =  new  SoPickStyle; 
unpickablestylenode->style. setValue  (  SoPickStyle: :UNPICKABLE  ); 

pickablestylenode->style. setValue  (  SoPickStyle: :BOUNDING_BOX  ); 
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//  create  the  terrain  box  <«««««<<«««« 

SoCube  *  backgroundCube  =  new  SoCube; 
backgroundCube->width  =  400.0, 
ba  rKground'.l'ube--^height  =  400.0, 
backgroundCube->depth   =    0.1 

SoTransform  *  xf backgroundCube  =  new  SoTransf orm; 
xfbackgroundCube->translation. setValue (0 .0,  0.0,  -50.0); 

ScT'rX!;ure2  *  overhangTexture  =  new  SoTexture2; 
overhangTexture->f ilename . setValue  ( "overhang . rgb" ) ; 

ScSeparator  *  sepbackgroundCube  =  new  SoSeparator; 
sepbackgroundCube--'addChild  (  pickablestylenode  ); 

.^ep;;.ackqroundCutae---addChi  Id  (  unitsfeet  )  ; 
sepc.ackgroundCube- -addChild  (  darkgreen  )  ; 
It  (TEXTURE  ==  TRUE) 

sepbackgroundCube->addChild (  overhangTexture  ); 
sepbackgroundCube->addChild (  xf backgroundCube  ) ; 

sepLackgroundCube->addChild (    backgroundCube  ) ;      //  remove  cube  here 
ror:  -  -addl'm  id  (  sepbacKqroundCube  )  ; 

//  Jason  with  engine  animation 

ScSeparator  *  sepJason  -    new  SoSeparator; 

ScTiansfcriT.  *   xf  Jason  =  new  SoTransform; 

SoSeparator  '    Jason  =  readFile  ( "Jason. iv" ) ; 

xfJason->translation .setValue (  -60.0,  40.0,  -  25.0);   //  center  of  pattern 

sepJason->addChi Id  (  xfJason  j; 

,' /  animation  from  Inventor  Mentor  13  .  6  .Calculator  .  C  +  + 

//  Set  up  the  Jason  transformations 

SoRotaticnXYZ   '  danceRotate  =  new  SoRotationXYZ; 

danceRotate-->anqle .  setValue  (  0.0  ); 

danceRotate->  axis . setValue  (  SoRotationXYZ :: Z  ); 

sepJason->addChild  !  danceRotate  i; 

ScTransiation  *  danceTranslate  =  new  SoTranslation; 

sep.rascn- --addChi  Id  {  danceTranslate  i; 

r-rpJason-'-addChj  id  (  Jason  ); 

roc:-"-'    addChiid  i  sepJason  ); 

//  Set  up  an  engine  to  calculate  the  motion  path: 

/  •  Theta  is  incremented  using  a  time  counter  engine, 

//  and  converted  to  radians  using  an  expression  in 

/ ,-  the  calcuiatci  engine. 

SoCaiculator   *  calcXY  =  new  SoCal culator ; 

ScT^meCounter  *  tnetaCounter  -    new  SoTimieCounter ; 

thetaCounter->max  =  360; 

thetaCounter->step  =  1; 

thetaCounter-'^f requency  =  1  /  180.0;  //  180  seconds  for  a  full  cycle 

calcXY->a  .  connectFrom  (£ithetaCounter-  >output )  ; 

calcXY->expression.setlValue(0,  "ta=a*M_PI /180 " 

calcXY->expression .setlValue (1 ,  " tb=15*cos ( ta ) " 

calcXY->expression .setlValue (2 ,  " td=tb*cos ( ta) " 

calcXY->expression . setlValue (3 ,  " te= tb'sin ( ta) " 

caicXY->expression . setlValue (4 ,  "oA=vec3f ( td, te, 

danceTranslate->translation.connectFrom(icalcXY->oA) ; 

calcXY->expression .setlValue (5,  "ob=2*ta");  //  scalar  output  b 

danceRot  a  te->angle.  connect  From  (S<calcXY->ob)  ; 


//  theta  (radians) 
//  r,  z 
//  X 

//  y 

tb)");  //  vector  output  A 


//  Platform 

SoSeparator  *  sepPlatform  =  new  SoSeparator; 
SoTransform  *  xfPlatform  =  new  SoTransform; 
SoSeparator  *    Platform  =  readFile  ( "Platform. iv" ) ; 
xfPlatform->translation.setValue(-80.0,  50.0,  -50.0); 
sepPlatf orm->addChild  (  xfPlatform  ); 
sepPlatf orm->addChild(  Platform  ); 
root->addChild  (  sepPlatform  ); 
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SoSepara^or  *  sepTesttank  =  new  SoSeparator; 
SoTransf orm  *   xfTestcank  =  new  ScTransf oriri; 
KoSeparacor  *    Testtank  =  readFile  ' "Test tank . iv" ) ; 
xfTesctank---CranslaLion.setValue  (  0.0,  0.0,  -50.0); 
sepTest!:anK->addChiid  (  xf  Test  tans  )  ; 
sepTtr?rtank--addC.nild(  Testtank  i  ; 
root ->addCni  Id  (  sepTesttank  /  ,- 

//  Torpedo  Tube 

ScSeparator  *  sepTorpedoTube  =  new  SoSeparator ; 
ScTrar.sfcrm  *   xfTorpedoTube  -   new  SoTransf  orm; 
xfTcrpedoTube->translation.setValue(  30.0,   20.0,  -48.00  ); 
£cO,/2:nder  *  TorpedcTube  =  new  SoCylinder; 
TorpedcTube->radius  =  21.0  /  12.0;       //  21"  diameter 
TorpedoTube->height  =10.0;  //  10'  length 

TorpedoT-jbe-->parts   =  SoOy'linder ::  SIDES;  //  let's  drive  through! 
sepTorpedoTube->addChild  (  xfTorpedoTube  ); 
sepTorpedoTube->addChild 1  TorpedcTube  ); 
root-  •addCh:Lld  (  brass  '; 
roct-'-addChild  ;  sepTorpedoTube  ); 

SoSeparator  »  Al^'_ncde  =  make.ALTV  i  )  ; 

root->addChild  (  AL^'_node  );  //  A'J\'  object  creation  routine  above 

,' ,  WriteAction  writes  scene  graph  to  file 

FILE  '  ajv_iv_fp  =  fopen  ("auv.iv",  "w";; 

SoWrireAzt ion  writeaction; 

wri'^i  re  :  or. .  getOutp-t  ,  ';---    .•^etFilePcinter    (auv_iv_fp)  ; 

wr:  teact :  on  .  apply    iAl^'.'_node;  ; 

f close      a^v.iv.fp^; 

ecu:  ■-  "wr:  :eact:on  .  appl\'  ( AL'^v'_ncde  )  =>  auv.iv  complete."  <<   endl ; 

//  WriteAction  writes  scene  graph  to  file 

FILE  *  auv_vw_:v_f  p  =  fope:-j  ( "  auv_vw  .  iv"  ,  "w";; 

wr: teact ion. getOutput () ->  setFilePointer  (auv_vw_iv_fp) ; 

writeaction. apply  (rocti; 

f cl G s e  •  a u v_'<'w_i \'_ f p  ; 

ccur  <■     "writeaction . apply  (root)  =>  auv_vw.iv  complete."  «   endl; 

DIS_net_open  ( ) ; 

atexit  (DIS_net_closey ;    //  ensure  port  is  reclosed  on  exit.   tested  sat. 

current_clock  =  clock  {};    II    initialize 

//  A  TimerSensor  updates  the  object  with  DIS  postures  and  performs  redraws 

ScTransfor.m    '  dummy_xform   =  new  ScTransf  orm; 

SoTimerSensor  *  DIS_Redraw_Sensor  =  new  SoTimerSensor (  DIS_Redraw_Callback, 

dummy _xform  ) ; 
L'IS_Redraw_Sensor->set  Interval  (  O.IC  );   //  seconds 
DIE_Redraw_Sensor->schedule  ij ; 

//  system  ("rm     sounds /nps_auv. au" ) ; 

//  system  ("www  -o  sounds /nps_auv.au 

file: //taurus.cs.nps. navy. mil /pub/auv/nps_auv .au" ) ; 

//  system  ("www  -o  sounds/nps_auv. au 

http: //www_tios . cs .utwente. nl /say /?Navai+ Postgraduate +School , Autonomous +Underwater+Veh 

icle" ) ; 

//  system  ("sfplay  sounds/nps_auv. au  &"); 

if  iPRINTDIALOG  ==  TRUE) 

//  Print  dialog  widget:   Inventor  training  manual  p.  9-9 
SoXtPrintDialog  *printDialog  =  new  SoXtPrintDialog; 
printDialog->setSceneGraph  (root) ; 
printDialoQ->show   (); 
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/'  cclcreditor  not  found?! 

if  (BACKGROUNDCOLORDIALOG  ==  TRUE) 
( 

/ /  Buiid  Che  color  editor  in  its  own  window 

ScXtColorEditor  *color_edi tor  =  new  SoXtColorEdi tor ; 
color_editor->build  (  )  ; 

color_editor->setTitle (  "AUV  viewer  background  color"  ) ; 
//  Add  a  callback  for  when  the  color  changes 

ccior_editor->addCclorChangedCallback (  colorEdi torCB,  //  the  callback 

viewer  ) ;  //  user  data  to  be  passed 

SbColor  lightbluecclor (  .0,  .5,  .75  ); 

viewer->setBackgroundColor (  lightbluecclor  ); 

color_editor->secColor     (  lightbluecclor  ) ; 

r-nlcr_editor-~-show ;  )  ;     //  Display  the  color  editor 


//  Uncomment  which  viewer  you  want  to  use: 

SoXrExarrdiierViewer  "  viewer  =  new  SoXtExaminerViewer ; 
//  SoXtFlyViewer      *  viewer  =  new  SoXtFlyViewer ; 

?"/!"  F2  jJ.^Viewei     *  viewer  =  new  ScXtPlaneViewer  ; 
/ '  EcXtWal KViewer     *  viewer  =  new  SoXtWalkViewer ; 

v,-rAtri-  -i^etScerieGraph  i    root    )  ; 

viewer-'-setTitle '  "NFS  AUV  Virtual   World"); 

viewer->snow ( ) ; 
. /  XcRealizeWidget  [    ViewerWindowWidget  ) ;   / /  mini  window  junk 

ScXt :  :iT,ainLoop  ( )  ;  //  loop  forever,  sending  events  to  the  scene  graph 
} 

/ /  end  of  viewer. C 
1 1 1 1 1 !  I  I :.'  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
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C.  Makefile  for  Object-Oriented  Real-Time  Graphics  Viewer 


« ! smake 
PROGRAM  =  viewer 
C*^FILES  -   viewer. C 

#  /usr/include/make/commondef s  and  /usr/include/make/commonrules 

#  define  some  useful  Makefile  rules.   For  example,  they 

#  define^^  a  'clean'  target,  so  you  can  type  'make  clean' 

#  to  remove  all  .o  files  (or  other  files  generated  during 

#  compilation).  See  the  file  /usr /include/make/commonrules  for 

#  do'iurrientation  on  wiiat  targets  are  supported. 

include  /usr /include /make/ commondefs 

TARGETS  =  S (PROGRAM) 

:-E-JE:T£  =  viewer,  c 

*>  LiDraries  to  link  with: 

LLE'LIEE  =    -llnventorXt 

#  1/lS   ^ncjudes 

IN':S  =  -  I  .  . /DIS  .mcast /h 

L:e_L'IR  =  -L.  . /DIS  .mcast/src 

LIB  -  . . /LIS .mcast /src/ libdis_client . a 

LIE_ARG  =  -ldis_client 

HDP._?ATK  =  ...  LjI£  .  mcast /]',/ 

HDPS  =  S ;HDR_PATH;pdu.h  S ( HDR_PATH ) disdef s . h 

default:  S (TARGETS: 

include  S (COMMONRULES) 

S (TARGETS):  S (OBJECTS) 

CC  S(C+-fFILES)  -O  $@  $(OBJECTS)  S(LDFLAGS)  $  (LIB)  $(LIB_DIR)  $  ( INCS )  \ 
S(LIB_ARG)   S(LLDLIBS) 

#  modified  from  pat  ton : /usr/share/src/Inventor/samples/clock/Makef ile 

#  updated   2  Octoijer  94   Don  Brutzman 
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V.   UNDERWATER  VIRTUAL  WORLD  HYDRODYNAMICS 

A.  Introduction 

Structuring  the  model  design  problem  was  the  key  to  comprehensible 
implementation.    A  straightforward  hierarchy  follows.   Posture  is  common  to  all 
vehicles  and  can  be  represented  either  by  Euler  angle  rotations,  by  a  homogenous 
transformation  matrix  (Fu  87)  (Foley,  van  Dam  90),  or  by  quaternions  (Cooke  92). 
Forces  and  accelerations  acting  upon  a  rigid  body,  if  modeled,  are  related  by  dynamics 
equations  of  motion  corresponding  to  each  spatial  degree  of  freedom.    A  rigid  body  is 
further  subject  to  kinematics  equations  of  motion  which  combine  velocities  with 
postures  in  strictly  defined  ways  regardless  of  vehicle  type  or  environmental 
dimensionality.    A  networked  rigid  body  which  communicates  with  other  entities  via 
DIS  needs  to  calculate  postures,  optional  linear  and  rotational  velocities,  and  (again 
optional)  linear  accelerations  (IEEE  93).   Such  a  DlS-networked  rigid  body  has 
identical  capabilities  regardless  of  vehicle  type. 

An  entity  dynamics  component  for  a  real-time  networked  virtual  world 
combines  the  functionality  of  rigid  bodies  and  DIS  networking  with  the  dynamics 
equations  of  motion  (forces  and  accelerations)  unique  to  a  specific  vehicle  type.   This 
structured  hierarchy  of  relationships  between  posture  representations,  rigid  bodies, 
DIS  networking  and  dynamics  equations  of  motion  led  to  the  general  model  class 
diagram  which  appears  in  Figure  1. 

The  compartment  boxes  within  Figure  1  delineate  the  functionality  of  class 
components.   The  first  compartment  is  class  name.   The  second  compartment  indicates 
member  data  fields,  which  are  the  data  structures  encapsulated  by  the  object.   The 
third  compartment  indicates  object  methods  (functions)  which  effectively  occur 
instantaneously.   The  fourth  compartment  includes  methods  (functions)  which  are 
time-consuming,  either  from  the  perspective  of  simulation  clock  duration  or  actual 
delay  due  to  network  latency.   Adapted  from  the  Object-Oriented  Simulation  Pictures 
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(OOSPICs)  design  and  testing  methodology  (Bailey  94),  this  diagraming  approach  is 
very  useful  because  it  simplifies  presentation  of  key  object  relationships  and  clarifies 
hierarchy  design.    Of  particularly  value  is  the  explicit  specification  of  temporal 
relationships,  which  are  critical  to  success  in  a  real-time  system  and  are  often 
overlooked  in  complex  system  design.   An  example  object  template  which  adapts  the 
OOSPICs  methodology  from  MODSIM  programming  language  to  C++  appears  as 
Figure  2.    A  key  for  OOSPIC  arrow  conventions  is  included  in  Figure  3  (Bailey  94). 
Although  C++  OOSPICs  are  not  provided  for  each  class,  inspection  of  specifications 
in  the  accompanying  source  code  reveals  that  every  class  follows  the  structural  layout 
presented  in  Figure  1. 
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Hydrodynamics  Model  Class  Hierarchy 


UUVBody 


body  accelerations 
mass  matrix 


permanent 
ownership 


initialize 

invert  mass  matrix 

integrate  equations  of  motion 


virtual  world  interface 
dynamics  respoiise 
dead  reckoning  response 


UUYModel 

hydrodynamics 
model  coefficients 


AUVSocket 

conmiimications  with 
networked  AUV 


K>f 


inherits 


pIS-NetworkedRigidBody 


DIS  port  connection  data 
Protocol  Data  Unit  (PDU) 
time  of  last  PDU 


initialize  and  inspection 
cotmect  and  disconnect 
send  PDU 


stay-alive  send  PDU 


temporary 
ownoshq) 


AUVglobals 

tdemetry  state  vector 
alternately  tq>dated  by 
robot  and  model 


y^J 


inhmts 


RigidBody 


quaternion  orientation 
h-transform-'oiatrix  posture 
posture:  position,  orientation 
body  velocities 


rotate  and  translate 

(st^  or  incremental) 
operators 

inspection  methods 
set  velocities 


tqxlate  posture  using 
velocities 


Not  shown:   Vector3D  class 


HTransformMatrix 


horizontal  transform 
matrix  with  posture 


Euler  angles 
rotate  aiid  translate 

(st^  or  incremental) 
operators 

inq>ection  methods 
set  posture 
camera,  scale  functions 


Quaternion 


q$  qi  Qi  qt 

(euler  parameters) 


rotate 

(st^  or  incremental) 
operators 
inspection  methods 


Figure  1.  General  real-time  DIS-networked  hydrodynamics  model  class  hierarchy. 
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Class  Name: 


inberits: 


Qcpui  pcmneten)     (letnni  type) 

deBcdptioD 

member  data  fields 

pnviie 

pubUc                                                                                                                                                                                         1 

instantaneous  methods 

piiv«te 

public 

coDsmictors 

opentots 

member  functions 

dcstmctor 

time-consuming  methods 

private 

■mntfitkm  clock  or  wsD  clock 

public 

Figure   2 . 


OOSPIC  class  diagram  template  for  C++ 
class  definitions.  Separation  of  class 
name,  data  fields,  instantaneous 
methods  and  temporal  methods  clarifies 
class  functionality  and  design. 


r^ 


i  i 


inberitancc     permanent     temporary     membershq) 
ownership     ownership 


Figure  3 . 


Object-Oriented   Simulation 
(OOSPICs)  arrow  conventions. 


Pictures 
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The  structure  of  the  general  real-time  DIS-networked  dynamics  model 
presented  here  appears  to  be  applicable  to  vehicles  of  arbitrary  type.   Documented 
source  code  for  each  member  of  the  class  hierarchy  matches  the  equations  and 
algorithms  presented  in  this  work,  and  also  follows  the  structure  appearing  in  Figure  2 
(Brutzman  94).    Future  work  of  interest  in  model  design  includes  directly  porting  this 
model  to  emulate  the  characteristics  of  other  underwater  vehicles,  adapting  the  model 
to  accommodate  dissimilar  vehicle  entities,  porting  the  model  into  robot  software  as  an 
on-board  hydrodynamics  response  predictor,  and  investigating  extensions  to  the  model 
to  support  visualization,  validation  and  verification  of  model  relationships  against 
archived  or  live  data  records  of  actual  vehicle  dynamics  performance. 
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B.  dynamics.C  Virtual  World  Interface  and  Top-Level  Hydrodynamics 

The  dynamics  program  is  the  server  connection  to  the  underwater  virtual 
world.   It  must  be  started  prior  to  the  robot  execution  program  so  that  the  robot  has  a 
virtual  world  to  connect  to.   dynamics  also  provides  a  user  interface  for  setting  ocean 
currents,  multicast  network  parameters  and  hydrodynamics  class  library  test  routines. 
The  user  menu  appears  in  Figure  4. 

The  dynamics  program  has  a  text-only  user  interface  in  order  to  reduce 
processor  loading,  maximize  portability  and  facilitate  remote  operation.   The  menu 
option  to  reset  multicast  group  parameters  permits  starting  a  virtual  world  channel 
using  collision-free  parameters  automatically  chosen  by  the  MB  one  sd  session 
directory  application.    The  menu  option  for  ocean  currents  accepts  a  constant  3D 
vector.   Extending  the  overall  functionality  of  the  underwater  virtual  world  is 
accomplished  by  building  hooks  between  new  models  (such  as  time-varying  ocean 
currents)  and  the  dynamics  program.   New  models  can  either  be  embedded  directiy 
within  dynamics,  or  can  be  distributed  and  communicate  via  sockets. 
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dude: /n/dude/work/brutzman/dynamics>>  dynamics 

Dynamics  classes 

test  selections 

L 

loop_test_with_execution_level  () ; 

M 

Multicast  parameter  input 

ttl=15,  group  address=224 . 2 . 121 . 93 ,  port=3111 

0 

Ocean  current  vector  reset 

<0,  0,  0> 

H 

Hmatrix/quaternion  exerciser 

R 

Rotation  of  quaternion  &  Hmatrix  using  p  q  r 

D 

Defaults 

I 

Invert  matrix  test 

E 

dEad_reckon_test_with_execution_level 

P 

PDU_skip_interval  change  (from  1) 

T 

Toggle  TRACE  =  0 

C 

DIS_net_close  ( ) ; 

Q 

Quit 

Enter  choice:   _ 

Figure  4.   dynamics  virtual  world  server:   user  interface. 


1 1 1  n  1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
I* 

Program:  dynamics. C 

Descripmon :  hydrodynamics  model  and  virtual  world  manager 

Author:  Don  Brutzman 

Revised:  18  October  94 

S>'stem:  Irix  5.2 

Compiler:  ANSI  C++ 

Compilation:  irix>  CC   dynamics. C  -Im  -w  -g  -o  dynamics 

[or]  unix>  make  dynamics 

+w  ==  Warn  about  all  questionable  constructs. 

Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Lessons:         Need  single  char  input  without  requiring  <CR> 

Don't  use  'static'  for  member  data  fields  or  mysterious  linker 
eRrors  result 
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Don't  use  <<  endl  <<  endi  more  than  once  in  a  single  cout  call 
or  a  vague  warning  results  with  an  invalid  line  number: 
'warning:   ostream&ostream:  :  operator  <<  (ostreains<  (  *  ) 
(oscreamSrj)  not  inlmed,  called  twice  in  an  expression' 


1 1 1  n  I  n !  1 1 1 1 1 1 1 1 1 1 1 1 1 .'  n  I  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

«inciudtr  <stdio.h> 
#include   <ctype.h> 

* i n elude    " UlTv'Body  . C " 

^include  <bstring.h> 

1 1 1 !  I  1 1 1 1 1  n  1 1 1 1 1 1 !  1 1 1 1  n  1 1 1 1 1 1 1 1 1 11 1 1 11 1 11 1 1 1 1 1 1 1 1 1 / 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
! .'    ciloLal  data 

char  port   I  6];    //  pointer  to  MULTICAST_PORT 

cnar  group  [20],-    //  MULTICAST_GROUP; 

int  int_ttl  =  15;  //  time-to-live 

char  choice  -    ' * ' ;   //  dummy  value  to  print  menu  first  time 

int  READ_MEI^IL'_CHOICE  =  TRUE; 

I ! n  i  f  i! i 1 1 1 1 1 1'!!  I  '  1 1 11 1 1 i 1 1 1 1 1 1 1 1 1 1 1 1 1 1  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 

void  parse_comiTiand_line_f lags    (int    argc,    char   **   argv) 

//  command  line  arguments 

int  index,  i ; 

//  3111  IS  npsnet  'standard'  port 
strncpy  iport,   "3111",    4); 
strnccy  'group,  "224.2.121.93",  12); 

••'/  cout  -■'■  "  lp3ri:e_comiT,and_line_f lags  start:  #  arguments  =  "  <<  argc 

//      <-.-  '■  ]  ■•  .  .  endl  ; 

//  cout  <■-     "   1  "  ; 

//  for  (I  =  0;  1  <  argc;  i++)  cout  <<  argv  [i]  <<  "  "; 

,'/  cout  «  "]"  <<-  endl; 

for  'i  =  1;  i  <  argc;  x**] 
{ 

tor    (index  =  0;  index  <=  strlen  (argv[i]);  index++)     //  uppercase 
arqv[i]  [index;  =  toupper  (argv[i]  [index]); 
} 

//  cout  «  "  [parse_comrriand_line_f lags  uppercase:  #  arguments  =  "  <<  argc 
//      «■]"<<  endl ; 

//  cout  «  " [   "  ; 

//  for  (i  =  0;  i  <  argc;  i++)  cout  «  argv  [i]  <<  "  ■; 
//  cout  «  ']"  <<  endl; 

for  (i  =  1;  i  <  argc;  i++) 
{ 

if      ((strcmp  (argv[i],  'TTL")  ==0)  II 

(strcmp  (argv[i],  "-TTL")  ==0)  II 

(strcmp  (argvli],  "T")  ==0)  II 

(strcmp  (argviii,  '-T")  ==  0)) 


{ 


if   (  i+1  >=  argc  ) 

cout  «  "Insufficient  parameters  for  TTL"  «  endl; 
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else 
{ 


int_ttl  =  atoi  {argv[ i+1 ] ) ; 

couc  <<  "dynamics:  tcl  reset  to  "  <<  int_ttl 
<<  endl; 


) 

else  if  ( (strcmp  (argv[i],  "PORT")  ==0)  II 

(strcmp  iargv[i],  "-PORT")  ==0)  II 

(strcmp  (argv[i],  "P")  ==0)  II 

(strcmp  (argv[i],  "-P")  -=   0)) 


{ 


if   (  i*l  >=  argc  ) 

cout  «  "Insufficient  parameters  for  PORT"  «  endl ; 
else 
( 

bzero   (port,   6) ; 

strcpy  (port,  argv[i+l)); 

cout  «  "dynamics:  port  reset  to  "  <<  port 
<•-  endl; 

i  +  -^  ; 


ei?e  if  ! (strcmp  ;argv[i] 

(strcmp  (argvlij 

(strcmp  (argv[il 

(strcmp  (argviij 

(strcmp  (argv[i] 

(strcmp  (argvlij 

(strcmp  (argv[i] 

(strcmp  (argviij 


"GROUP" !  ==  0) 

"-GROUP")  ==  0) 

"G")  ==  0) 

--G";  ==  0) 

"ADDRESS")  ^^  0) 

"-ADDRESS" )  ==  0) 

"A" )  ==  0) 

"-A")  ==  0) 


It   i 

else 
{ 


(  i+1  >=  argc  ) 
cout  <<  "Insufficient  parameters  for  GROUP  ADDRESS"  «  endl  ; 


bzero   (group,   20)  ; 

strcpy  (group,  argv[i+lj;  ; 

cout  «    "dynamics:  group  address  reset  to  "  <<  group 

<<  endl; 
i  ++ ; 


i 


'LOOP" )  ==  0)  II 

■LOOP")  ==0)  M 

"L")  ==0)  M 

"-L")  ==  0) ) 


) 

else  if  ((strcmp  (argv[i], 

(strcmp  (argv [ i  j , 

(strcmp  largvli] , 

(strcmp  (argv[ i ] , 

{ 

choice  =  ' L' ; 
READ_MENU_CHOICE  =  FALSE; 

cout  <<  'dynamics:  Loop  test  with  execution  level  set  "  «  endl; 
) 

else  if  ((strcmp  (argv[i],   "TRACE")  ==0)  II 
(strcmp  (argv[i],  "-TRACE")  ==0)) 

{ 

TRACE  =  TRUE; 

cout  <<  "dynamics:   TRACE  is  now  on"  «  endl; 


eise 
{ 

} 


cout  <<  "dynamics:   unrecognized  command  flag  "  «  argv[i]  <<  endl; 


//  end  for  loop  through  command  line  parameters 
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//   cour    <-<    "  lparse_cominand_l  ine_f  1  ags   connplece]"    <<  endl  ; 
I■ecur^l  ; 

;•  //  end  p3rse_command_line_f lags  () 

1 1 1 1 !  I  1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
int  argc,  char  **  argv  ) 


vcid  main 

V 

double 


mc 

inc 

Vector  3D 

Quaternion 

Hmar  rix 

RigidBody 

LISNetworkedRigidBody 

UUVBody 


al, 

bl  , 

cl, 

xi , 

yi- 

zl, 

p. 

q. 

r. 

delta_time; 


done  ^  FALSE; 
index,  repetitions; 

vectorl,  vector2; 

quaternionl,  quaternion2; 

hmatrixl,    hinatrix2; 

rigidbodyl ; 

disnetworkedrigidbodyl ; 

uuvbodyl ; 


//   ir.arn    start 

r..a:  se_:or'L';^and_l:ne_f  lags  (argc,  argv); 


ac 


ccut 

COJt 

ccut 

COUt 

ccut 
cout 

ccut 

cout 
cout 


cout 
ccut 


isspace  f choice) 
endl ; 


-.    endl; 

•'    "Lvnarrac 


s  classes  test  selections" 

L  loop_test_with_execution_level  ();' 
M  Multicast  parameter  input" 

ttl="  <<  int_ttl  «  ",  group  address^" 
",  port="  <<  port 
0  Ocean  current  vector  reset  " 
<-•■  <<  AUV_oceancurrent_u  <<  " ,  " 
«   AUV_oceancurrent_v  «  ",  ' 
<<  AUV_oceancurrent_w  «  ">  " 
H  Hmatrix/quaternion  exerciser" 
R  Rotation  of  quaternion  &  Hmatrix  using 


cout  <<    " 

cout  <<  " 

cout  <<    " 

cout  «  " 

« 

cout  «    " 

cout  <<  " 

cout  <<  " 

cout  «  "Enter  choice 

cout  <<  flush; 


D  Defaults" 
I  Invert  matrix  test" 

E  dEad_reckon_test_wi th_execution_level " 
P  PDU_skip_interval  change  (from  " 
uuvbodyl . PDU_skip_interval_value  {)  <<  •)■ 
T  Toggle  TRACE  =  "  «  TRACE 
C  DIS_net_close  ( ) ; " 
Q  Quit" 


«  endl 
<<  endl 
<<  endl 
<<  group 
<<  endl; 
<<  endl; 


«  endl 
«  endl 
p  q  r  " 
«  endl 
«  endl 
«  endl 
«  endl 

«  endl 
«  endl 
«  endl 
«  endl 


if  (READ_MENU_CHOICE  ==  TRUE) 
\ 
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//  still  not  reading  a  single  char  properly  : ( 

/ /  cin. get  (choice; ; 

cm      »   choice; 

cout  <<  endl; 
i 
else  cout  «  choice  «  endl ; 

REAEi_MENU_CHOICE  =  TRUE; 

switch  (choice) 

case  'L':   case  '1':  // 

cout  <<  "Loop  test  with  execution  level"  «  endl; 

uuvbodyl .UUVBody_initialize  (); 

uuvbodyl . set_ttl  (int_ttl); 

uuvbodyl . set_group  (group); 

uuvbodyl . set_port  (port); 

cout  «  'dynamics:  uuvbody . loop_test_wi th_execution_level  ()"  <<  endl, 

uuvDody 1 . loop_test_wi th_execution_level  () ; 

chrice  =  '*';   //  dummy  value  to  print  keyboard  menu 
breaK ; 


'0 ' :   case  ' C  :  //• 


cout    ■----■    "    Ocean   current    vector   reset    "    «   endl; 

co'Jt    '■•■    " "    <<   endl; 

cout    <-■<   endi  ; 

cout    <<    "Enter  AUV_oceancurrent    m   North   direction: 

cm      "'-•   Airv_oceancurrent_u ; 

cout    <■<•    "      new  Air>/_oceancurrent_u    =    "    <<  AUV_oceancurrent_u   <<   endl; 

cout    <<    end!  ; 

cout  «  "Enter  AUV_oceancurrent  in  East   direction:     "; 

cm   ~'".-  AU\'_oceancurrent_v; 

cout  ■--'  "   new  AU\'_oceancurrent_v  =  "  <<  AUV_oceancurrent_v  <<  endl; 

cout  <<   endl; 

cout  <•<  "Enter  AUV_oceancurrent  in  Downward  direction:  "; 

cin   >■>  AUV_oceancurrent_w; 

cout  •'■'  "   new  AUV_oceancurrent_w  =  "  <<  AUV_oceancurrent_w  <<  endl; 

ccut    ■■•■■   endl; 

choice  =  '*';   //  dumm>'  value  to  print  keyboard  menu 
break; 


case  'M' :   case  'm' :  //■ 


cout  «  "  Multicast  parameter  input  "  «  endl; 

cout  <<    " "  <<  endl; 

cout  '-^  endl; 

cout  «  "Enter  time  to  live  ttl : 

cin  »  int_ttl; 

cout  <<   "     new  time  to  live  ttl  -    "  <<  int_ttl  «  endl; 

cout  <<r   endl; 

cout  <<    "Enter  group  address: 

cin  »  group; 

cout  «  "   new  group  address    =  "  <<  group  «  endl; 

cout  <<  endl; 
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couc  <<  "Enter  port 
cin  »   port; 
couc  <<  "   new  port: 
cout  --'-  endl  ; 


choicer 
break ; 


=  "  <<  port  <<  endl ; 
■'/  duminy  value  to  print  keyboard  menu 


case  'H':   case  'h':  // 

cout  •.--■  "Hmac  rix/ quaternion  exerciser:"  <<  endl  ; 

cout  «  "Enter  euler  angles  in  degrees  for  object: 

cin   >>  al  >>  bl  >>  cl ; 

cout  <<-    "    angles  entered  =  <"    «   al  <<  " ,  "  <<  bl  << 

-  -^  endl  ; 


"  <<  cl  <<  ">' 


quaternionl  =  Quaternion  (radians  (al),  radians  (bl),  radians  (cl)); 

cout  -•  "quaternionl: 
quaternionl .print  ( ) ; 
quaterruonl  .  normal  i  ze  (  )  ; 


cout  «   enai ; 

cout  <<  "normalized : 


<<  q-uaternionl  <<  endl. 


ve7tor2  =  quaternionl . euler_angies  ( ) ; 

cout  <<  "euler  anglesl:  "  <<  vector2  <<  endl ; 

cout  «    "phi  theta  psil:"  <<  quaternionl .phi_value    ()  <<  ",  " 

<<  quaternionl . theta_value  ()  <<  ",  " 

<<-  quaternionl  .psi_value    ()  «   endl  ; 

ccL.r  <■•     "degrees:        "  <<  degrees  (quaternionl  .phi_value  ();  <<  ",  " 

<<  degrees  (quaternionl . theta_value  ())  <<  ",  " 

<<  degrees  (quaternion! .psi_value  ())  <<  endl 
end!  ; 


c  ^u  t 


cout 
cin 


ve;t: 
cout 


"Enter  vector  positions  for  object 
xl  >■■  y  1  -•■>  21  ; 


:  Vector3D 
■vertcrl : 


( X 1 ,  y  1 ,  2  :  )  ; 

"  <<  vectorl  «  endl; 


hn-iatrixl  =  Hmatrix  (  vectorl,  radians  (al),  radians  (bl),  radians  (cl)) 
cout  <'-  "hmatrixl:       "  <<  hmatrixl  <<  endl; 


CCJt 


clicice  = 
break ; 


hmatrixl  angles:  "  <<  degrees  (hmatrixl .phi_value  ())  <<  ",  " 

<<   degrees  i  hiriatrixl  .  theta_value  ())  <<  ",  " 

«   degrees  (hmatrixl .psi_value  ())  «  end! 

'*';   //  dummy  value  to  print  keyboard  menu 


case 


case' r ' : 


II- 


cout  «    "Rotation"  «  endl; 

cout  «  endl; 

cout  <<  "Enter  euler  angles  in  degrees  for  object:       •; 

cin   »  al  >>  bl  »  cl; 

quaternionl  =  Quaternion  (radians  (al),  radians  (bl),  radians  (cl)); 

cout  «  "Enter  rotation  rates  in  degrees  per  sec  <p,  q,  r>: 
cin  »  p  >>  q  >>  r  ; 
p  =  radians  (p) 
q  -   radians  (q) 
r  =  radians  (r) 
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COL.C  •:■-  "Enter  repetitions  and  delta_tinie  in  seconds: 
cin   ■^>  repetitions  >>  delta_time; 

cout  <<  "Enter  vector  positions  for  object: 
cin   >>  xl  >>  yl  »  zl; 

vector!  =  Vector3D  {xl,  yl,  zl); 

hmatrixl  =  Hmatrix  (vectorl,  radians  (al),  radians  (bi; 
hiTatrix2  =  Hmatrix  (vectorl,  radians  (al),  radians  (bl, 

for  (index=0;  index  <=  repetitions;  index++) 


radians  (cl ) ) 
radians  (cl ) ) 


cout  <<    "quaternion  =  "; 

quaternionl .print  (); 

cout  «  end! ; 

cout  «  "quaternionl  angles  =  <" 

<'-  degrees  (quaternionl  .phi_value 
«   degrees  (quaternionl . theta_value 
«  degrees  (quaternionl .psi_value 

quaternionl . incrementaI_rotate  (p,  q,  r. 


0  )    «    "  ,     " 

(  )  )    «    "  ,     " 

( ) )    «    ">    "    «   endl ; 

delta_time) ; 


cout    <■ 


hmatrixl  angles  =  <"  «  degrees  (hmatrixl .phi_value  ()) 

<<  ",  "  <<  degrees  (hmatrixl . theta_value  ()) 

<<  " ,  "  <<  degrees  (hmatrixl .psi_value  ()) 
«    ">    "    «  endl; 


cout  <<  "    hmatrix2  angles  =  <' 

<<  "  ,  ' 

<<  "  ,  ' 

<<  "  >  ' 


«  degrees  (hmatrix2 .phi_value  ()) 

<<  degrees  (hmatrix2 . theta_value  ()) 

<<  degrees  (hmatrix2 .psi_value  ()i 
<<  endl; 


c  ou  t 


"hmat  rixl 


hmatrixl  <<  endl; 


/ /  two  alternate  methods  can  be  tested: 

hmatrixl  .  incremental_rotation    (p,    q,    r,    del  ta_timiei  ; 

hmatrix2 . rotate  (p  *  delta_time,  q  *  delta_time,  r  *  delta_time); 


cout  <<   

cin. get  (cncice;;  //  pause 


«  endl , 


cno ice  = 
break ; 


//  dummy  value  to  print  keyboard  menu 


case 


'd':  //■ 


cout  <<  "Defaults"  «  endl 
cout  <•-  endl; 


vectorl  =  Vector3D  () 
quaternionl  =  Quaternion  () 
hmatrixl     =  Hmiatrix     () 

cout  «  "Using  «  operators:"  <<  endl; 

cout  <<  "Default  vector3D:  "  <<  vectorl     <<  endl 

cout  <<  "Default  quaternion:  "  «  quaternionl  <<  endl 

cout  «  "Default  Hmatrix:  °  «  hmatrixl; 


cout  <--'  "Using  print  methods:"  «   endl; 

cout  «  "Default  vector3D: 

vectorl .print  ( ) ; 

cout  «   endl  «  "Default  quaternion: 

quaternionl .print  (); 

cout  <<•  endl  «  "Default  Hmatrix: 

hmatrixl .print_hmatrix  (); 

cout  «  endl; 

cout  <<  "initializations  &  assignment:" 


<<  endl ; 
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vectorl      =  Vector3D    (1,  2,    3); 

cout        <<  "  VeccorBD    (1,  2,  3)  ==  "  <<  vectorl  «  endl ; 

quaternionl  =  Quaternion  (0,  0,  0); 

cout       <<  "  Quaternion  (0,  0,  0)  ==  "  <<  quaternionl  «  endl ; 

hmatrixl     =  Hmatrix  (  4C,  50,  60, 

radians  (Oi,  radians  (-90),  radians  (0)  ) ; 

cour       -^<  "  Hmatrix  (  40,  50,  60,  radians  (0),  radians  (-90 ),  " 

<■'  '    radians  (0))  "  <<  hmatrixl  <<  endl  ; 

hiTiBtrixl  =    Hmatrix    {Vector3D  (40  ,  50  ,  60)  , 

Vector3D(radians (0) ,  radians (-90 ) ,  radians (0) )) , 
cout        «  "  Hmatrix  (Vector3D (40 ,  50,  60)," 

«  "  Vector3D(radians  (0),  radians (-90 ) ,  radians (0)))  " 

•-^'  hm.atrixl  <<  endl  ,- 

quaternionl  =  Quaternion  (radians  (90),  0,  0)  * 

Quaternion  (0,  0,  radians  (90)); 
cout  <<  "quaternion  mul tipl icaton  test  <90,0,0>  *  <0,0,90>  =>"  <<  endl 
cout  <<  "  <"  <<  degrees  (quaternionl .phi_value    ())  «  ",  ' 
«r<  degrees  (quaternionl  .  theta_value  ())  «  ",  ' 
«-<  degrees  (quaternionl  .psi_value    ())  «  ">  "  «  endl; 

ri'j:dL)Odyl  =  RjgidBody  (); 

cout  -  --  endl  ; 

cout  <<  "  RigidBod;>'  ()  defaults  printed  using  different  methods:" 

<<    rigidbo&y-l ; 
rigidbodyl  .print_rigidbody  (;; 
cout  <<   endl; 

disnetworkedrigidbodyl . DISNetworkedRigidBody_initiali2e  ( ) ; 
cout  <<  "  DISNetworkedRigidBody  ()  defaults  printed  using  " 

<<•  "different  methods:"  «  disnetworkedrigidbodyl; 
disnetworkedrigidbod>'l  .print_networkedrigidbody  ()  ; 
couc  •■--  endl; 

cout  ■■<    "   UUVBody  ()  defaults  printed  using  different  methods:" 

•  •■  uuvbodyl ; 
■juvbcdy  1  .  prinr_uuvbody  '}  ; 

ch"ic«^  =  '  *  '  ;   //  dummy  value  to  print  keyboard  menu 
brean ; 

rase  'I':   case  'i':  // 

cout  <•■  "Invert  test:   uuvbody .  test_invert_matrix  ()"  <<  endl; 

uuvbodyl . test_invert_matrix     ( ) ; 

choice  -    '*';   //  dummy  value  to  print  keyboard  menu 
break ; 


case  'E':   case  'e':  // 

cout  <<r    "dEad  reckoning  test"  <<  endl; 

cout  '-<  "uuvbody  .dead_reckon_test_with_execution_level  ():"  <<  endl; 

uuvbodyl .dead_reckon_test_with_execution_level  () ; 

choice  =  '*';   //  dummy  value  to  print  keyboard  menu 
break; 


case  'T' :   case  ' t ' :  //• 


if     (TPuBlCE   ==    0)    TRACE   =    1; 
else  TRACE   =    0; 
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coMt    <<    "Trace  toggled,  now  TRACE  =  "  <<  TRACE  << 

if  (TRACE  ==  0)  COUC  «  "FALSE)"; 
else  cout  «  "TRUE)"; 

break ; 

case  'P':   case  'p':  // 


cout  ".'<  "PDU_skip_intervaI  change  (from  " 

<<  uuvbodyl .  PDU_sk.ip_interval_value  ()  «  ")"  <<  endl ; 

cout  <<    " PDUs  will  only  be  sent  every  <value>  tenths  of  a  second." 

'-<  endl; 
cout  «    "Values  less  than  10  may  make  applications  prone  to  crash.' 

<<  endl; 
cout  <-<  "Enter  new  value: 

int      new_skip_value; 
cm   >>   new_skip_value; 
cout    -■-<   endl; 

uuvbody 1 . set_PDU_skip_interval  (new_skip_value) ; 

break; 

ist  ' C ' :       case  'c':  // 

■rout  -.--  "Close"  <•■•  endl; 

cout  •-■■-;  "DIS_net_close  ()"  <•-  endl; 

uuvbody 1 . DI£_net_close  { ) ; 

break  ; 


ca~tr    'Q':      case    'q':    //-- 

cout  •-<  "Quit"  «  endl; 
dene  =  TRUE; 
break ; 

default:  //-- 


if  (  ]  isspace  (choice))  cout  <<  "  ?  unknown  option.."  «  endl; 
treak ; 

}  //  end:   switch  (choice) 

)  while  (done  ==  FALSE) ; 

//  normal  exit 
) 
i  1 1 li  i 1 1 1 1 1 1 1 1 1 n II 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 / 1 1 1 1 1 1 n 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
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C.  AUVglobals.h  Robot  Telemetry  Variables 

1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 11 1 1 1 1 1 1 1 1 1 / 1 1 1 1 1 1 1 1 1 1 1 1 1 ! 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

Header  file:  AlT\'globaIs  .  H 

Author:  Don  Brurzman 

Revised:  21  October  94 

System:  Irix  5.2 

Compiler:  ANSI  C++ 

Ccmi::  lat  lor- :  irix-  make  dynamics 

r)es~r:pr  ion :     Generalized  vehicle  state  variables,  customized  for 

NFS  AUV  II  operation  with  virtual  world  hydrodynamics  model 

Adv:.?or:?:        Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 


^  y  ^•-  ~  c-r. 


Brutzman,  Donald  P.,  "Integrated  Simulation  for  Rapid 
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#  i  f  ndef  A'.t\^gL0BALE_H 

#defin«r  A;.T1L0BA.LS_H    /'/  prevent  errors  if  multiple  #includes  present 

/*  AUV  telemeti-y  state  vector                                            */ 

*  / 

,'■'  Note  these  are  globals  for  direct  access  by  any  world  model.   Refer  to     */ 

/*  individual  world  models  for  details.   Data  hiding  within  a  private  object   */ 

/*  is  not  necessary  since  all  values  are  transient  and  superseded  by  actual    */ 

/*  state  when  it  occurs.   Additionally,  half  of  the  state  variables  are       */ 

/*  provided  only  by  the  AUV  microprocessor  socket,  and  the  other  half  are     */ 

/*  provided  by  respective  world  models.   Thus  global  variables  in  this  design  */ 

/*  are  not  vulnerable  to  corruption  and  side  effects,  making  data  hiding      */ 

/*  unnecessary.  */ 

/*««***«■****«*******«**«*««**«**««***********«***««*********»*******«**********/ 
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AW-provided  state  variables 


static  double  AUV_tiine 
static  double  AL'V_del  ta_ruddGr 
static  double  Airv'_delta_planes 
static  double  AlTV_port_rpin 
static  double  AUV_stbd_rprr 

static  double  AUV_bow_vertical 
static  double  Airv'_sterri_vertical 
static  double  AUV_bow_lateral 
static  double  AUV_sterr,_lateral 

static  double  AUV_depth_cell 
static  double  AU\'_heading 
static  double  AUV_roll_angle 
static  double  AU\''_pitch_angle 
static  double  AUV_roll_rate 
static  double  AUV_pitch_rate 
static  double  AU\'_yaw_rate 


static  int 
static  int 
static  int 


AlPv_hour 
AUV_minute 
AUV  second 


0 

0 

0 

0 

0 

0 

G 

G 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

//  mission  time 

//  positive  is  bow  rudder  to  starboard 

//  positive  is  bow  planes  to  starboard 

//  propeller  revolutions  per  minute 

//  propeller  revolutions  per  minute 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  -   3820  rpm  no  load 

//  pressure  sensor,  units  are  psid 

//  gyrocompass  in  degrees 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  internal  AUV  OS-9  system  time,  unused 


Hydiodynamics-iTiodel-provided  state  variables 


static  douDle  AUV_x  =  0.0 

static  double  AUV_y  =  0.0 

static  double  AU"v'_2  =  0.0 

sta:i~  double  AU\''_phi  =  0.0 

stii::r  JoL-ce  AU"v'_tt"ieta  =  C.O 

static  douDle  AU\'_jJsi  =  C.O 

static  double  AUV_x_dot  =  0.0 

static  double  AiJV^_dot  =0.0 

statir  double  AU'\/_z_dot  =  0.0 

static  double  AU'\/_phi_dot  -  0.0 

static  double  AU'/_theta_dot  =  0.0 

sraric  double  AU"\/_t;)Si_dot  -  0.0 

static  double  AU\'_u  =  0.0; 

static  double  AUV_v  -  0.0; 

static  double  AUA/'_w  =  0.0; 

static  double  AUV_p  =  0.0, 

static  double  AUV_q  =  0.0, 

static  double  AUV_r  =  0.0; 

static  double  AUV_u_dot  =0.0 

static  double  AUV_v_dot  =0.0 

static  double  A\T-J_vi_doz  =  0.0 

static  double  AUV_p_dot  =  0.0 

static  double  AUV_q_dot  =  0.0 

static  double  AUV_r_dot  =0.0 

static  double  AUV_oceancurrent_u  =  0.0, 

static  double  AUV_oceancurrent_v  =  0.0; 

static  double  AUV  oceancurrent  w  =  0.0, 


/ /  X    position  in  world  coordinates 

//  y.   position  in  world  coordinates 

//  z    position  in  world  coordinates 

//  roll   posture  in  world  coordinates 

//  pitch  posture  in  world  coordinates 

//  yaw   posture  in  world  coordinates 

//  Euler  velocity  along  North-axis 

//  Euler  velocity  along  East-axis 

//  Euler  velocity  along  Depth-axis 

//  Euler  rotation  rate  about  North-axis 

//  Euler  rotation  rate  about   East-axis 

/ /  Euler  rotation  rate  about  Depth-axis 

//  surge  linear  velocity  along  x-axis 

//  sway   linear  velocity  along  y-axis 

//  heave  linear  velocity  along  x-axis 

//  roll   angular  velocity  about  x-axis 

//  pitch  angular  velocity  about  y-axis 

//  yaw   angular  velocity  about  z-axis 

//  linear  acceleration  along  x-axis 

//  linear  acceleration  along  y-axis 

//  linear  acceleration  along  x-axis 

//  angular  acceleration  about  x-axis 

//  angular  acceleration  about  y-axis 

//  angular  acceleration  about  z~axis 

//  Ocean  current  rate  along  North-axis 

//  Ocean  current  rate  along   East-axis 

//  Ocean  current  rate  along  Depth-axis 
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//  Prior  time  loop  saved  variables 


scacic  double  AUV_t ime_prior 

scacic  double  AUV_x_prior 

static  double  AUV_y_prior 

static  double  ALnv'_z_prior 

static  double   AW_phi_prior 

static  double  AUV_theta_prior 

stst:c  double  AUV_psi_prior 


0.0;  //  mission  time 


0.0 
0.0 
0.0 

0.0 
0.0 
0.0 


//  X    position  in  world  coordinates 

//  y    position  in  world  coordinates 

//  z    position  in  world  coordinates 

//  roll   posture  in  world  coordinates 

//  pitch  posture  in  world  coordinates 

//  yaw   posture  in  world  coordinates 


/   Sor.ar  model  provided  state  variables 

static  double  AUV_ST1000_bearing  =0.0 

static  double  AUV'_£:T1000_range    =0.0 

sraTi:  double  AbT_£:Tj  00  0_strenQth=  0.0 

r-:r.j:t   double  AiJv'_s:T100C_x_cf  f  3et=  3.5 

stati:  double  A'-j;_ST7  2  5_bearing   =0.0 

srat-c  double  AUv'_£T7  2  5_range    =0.0 

static  double  AUV_£'T7  2  5_strength  =  0.0 

const   double  AUV  ST725  x  offset  =3.5 


//  ST_1000  conical  pencil  beam  bearing 

//  £T_1000  conical  pencil  beam  range 

//  ST_1000  conical  pencil  beam  strength 

//  ST_100G  longitudinal  location  in  feet 

//  ST_725  1  X  24   sector  beam  bearing 

//  ST_7  2  5  1  X  24   sector  beam:  range 

//  ST_725  1  X  24   sector  beam  strength 

//  ST_725  longitudinal  location  in  feet 


Wr, 


■wide  global  variables  and  constants 


statii 


TRACE 


/'  1  =  trace  on,  0  =  trace  off  ' / 


#defi.ne  TRUE   1 
#define  FALSE  0 

#define  PI     3.1415926535697932 


*■*■»•«*■»*««*■**■»■■ 


■*********««««»-«**X********' 


»/ 


«er,d,f      //  endif 


167 


D.  VUVModelh  Hydrodynamics  Model  Coefficients 

I  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

Prograrr;:  UU^/model  .H 

Author:  Don  Brutzman 

Revised:  26  October  94 

System:  Irix  5.2 

Compiler:  ANSI  C+^ 

Compii ar icn:     irix>  make  dynamics 

irix-  CC  UUVmodel.H  +w 

+w  ==  Warn  about  all  questionable  constructs. 

Debugging:       grav>'l :  ~brutzman/dynamics>>  lint  UUVmodel.h 

Advisors:        Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

References:      Healey,  Anthony  J.  and  Lienard,  David,  "Mul tivariable 

Sliding  Mode  Control  for  Autonomous  Diving  and  Steering 
of  Unmanned  Underwater  Vehicles,"  IEEE  Journal  of  Oceanic 
Engineering,  vol.  16  no.  3,  July  1993,  pp.  327-339, 

Lewis,  Edward  V.,  editor,  ^Principles  of  Naval 
Architecture  volume  III_,  second  revision.  The  Society  of 
Naval  Architects  and  Marine  Engineers,  Jersey  City 
New  Jersey,  1968,  pp.  188-190  and  418-423. 

Gertler,  Morton  and  Hagen,  Grant  R.,  _Standard  Equations 
of  Motion  for  Subm.arine  Simulation_,  Naval  Ship 
Research  and  Development  Center  (NSRDC)  Research  and 
Development  Report  2510,  Washington  DC,  June  1967. 

Smith,  N.S.,  Crane  J.W.  and  Sumimey ,  D.C.,  _SDV  Simulator 
Hydrodynamic  Coef f icients_.  Naval  Coastal  Systems  Center 
(NCSC),  Panama  City  Florida,  June  1978.   Declassified. 

Marco,  David.   "Slow  Speed  Control  and  Dynamic  Positioning 

of  an  Autonomous  Vehicle,"  Ph.D.  dissertation, 

Naval  Postgraduate  School,  Monterey  California,  March  1995. 

Bahrke,  Fredric  G.,  "On-Line  Identif icaton  of  the  Speed, 
Steering  and  Diving  Response  Parameters  of  an  Autonomous 
Underwater  Vehicle  from  Experimental  Data,"  Master's  Thesis, 
Naval  Postgraduate  School,  Monterey  California,  March  1992. 

Warner,  David  C,  "Design,  Simulation  and  Experimental 
Verification  of  a  Computer  Model  and  Enhanced  Position 
Estimator  for  the  NPS  AUV  II,"  Master's  Thesis, 
Naval  Postgraduate  School,  Monterey  California,  December  1991. 

Notes:  const  definitions  are  for  software  engineering  reliability 

they  can  be  changed  to  variables  if  coefficient  modification 
becomes  desirable 

V 
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«ifndef  UUVMODEL_H 

#define  UUVMODEL_H    //  prevent  errors  if  multiple  #includes  present 
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//  #define  £1 


//  <<«<<««««<<  uncomment  this  statement  for  SI  units 
//  otherwise  standard  British  units  used 


■// 
■// 


term 


value 


units 


description 


#ifdef  SI 

const  double 

const  double 

const  double 
const  double 
const  double 
cc;n.;t  double 

«define 


const  aoucje 
cor.s:  double 
corjSL  double 
zoi.st  d&L-Lle 
const  double 
ccTiSu    double 


//  Systeme  International  (metric)  units 


Weight   =  435.0*0.454, 

//  =  197.4  9 
Buoyancy=  435.0*0.454, 


// 

L 

g 

rno 

m 

// 

m4  f t4 


I_x 

i_y 

llxy 
I  xz 


=  197.49 

2.23 

9.81 
=  lOOC.O 
=  Weight  /  g 
20.131498 


//  N  Weight    (0.454  kg/lb  ==  1) 

//  N  Buoyancy  (0.454  kg/lb  ==  1) 

//  m  characteristic  length  87.625" 

//  m/s^2  gravitational  constant 

//  kg/m^3  mass  density  of  fresh  water 

//  N-s'~2/m  vehicle  mass  incl  .  free  flood 


!0. 305) *  ("P. 305) * (0.305) * (0.305)  //  (0.305  m/ft  =-    1, 


2 .7*m4_ft4 
2  .7*in4_f  t4 
2 .7*m4_f t4 

0.0 

0.0 

0.0 


//  Inertia  matrix  coefficients 

//  Nms"2  =  I_xx  =  0.023364857 

//  Nms'-2  =  I_yy  =  0.023364857 

//  Nms^2  =  I_ZZ  =  0.023364857 

/■  /  Nms  '2  =  I_yx 

//  Nms'"2  =  I_zx 

//  Nms "2  =  I_zy 


*undef 

const  double 

const  double 

const  double 

const  double 

const  douLle 

const  dcut'le 


m4_f c4 
x_G 

y_G 

z_G 
x.B 

y_B 

z  h 


// 

0 

0 

// 

cm 

0 

0 

// 

cm 

6 

1 

// 

cm 

0 

0 

// 

cm 

0 

0 

// 

cmi 

0 

0 

// 

cm 

Centers  of  Gravity  &  Buoyancy 


Note  CG  below  CB   Marco  0.5' 


CB  at  center  of  UUV 


#else  //  (not  SI)  standard  British  units 


const  double 

const  double 

const  double 

const  double 

const  double 

const  double 


Weight 

Buoyancy= 

L 

g 

rho     : 


435.0 

435.0 

=  7.30208333 

32.174 

1  .94 

=  Weight  /  g 


//  lb  Weight 

//lb  Buoyancy 

//  ft  characteristic  length  87.625" 

//  ft/s^2  gravitational  constant 

//  slugs/ft'"3  mass  density  of  fresh  water 

//  lb/ft-s''2  vehicle  mass  incl.  free  flood 


//  = 


13.520234 


const 

double 

I   x 

const 

double 

I_Y 

const 

double 

I_z 

const 

double 

I_xy 

const 

double 

I    xz 

const 

double 

I^z 

const 

double 

X    G 

const 

double 

Y_G 

const 

double 

Z_G 

const 

double 

X    B 

const 

double 

y_B 

const 

double 

z    B 

2.7 
42  .0 
4  5.0 
0.0 
0.0 
0.0 


0.125/12.0 

0.0 
1.07  /12.0 
0.125/12.0 

0.0 

0.0 


// 


Inertia  matrix  coefficients 


//  lb-ft-sec"2ft'^4 

//  ft'^4      =i_yy 

//  ft"4       =I_zz 

//  ff"4       =I_yx 

//  ft"4        =I_zx 

//  ft'-4       =I_zy 


:I   XX 


// 

//  ft 

//  ft 

//  ft 

//  ft 

//  ft 

//  ft 


Centers  of  Gravity  &  Buoyancy 
0.010416667 

Note  CG  below  CB 
0.010416667 

CB  at  center  of  UUV 


//  Thruster/propeller  distances  from  centerlines.   Note  stern/port  are  negative. 
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const  double 

const  double 

const  double 

const  double 


x_bow_vertical 
x_stern_verticai 
x_bow_lateral 
X  stern  lateral 


1.41 
1.41 
1.92 
1  .92 


//  ft  Marco  17"  measured  13.0' 

//  ft  Marco  -  17"  measured  -  21.0' 

//ft  Marco  23"  measured  19.0' 

//  ft  Marco  -  23"  measured  -  26.0' 


ccr:st   double     y_port_propei  ler 
const   double     y_stbd_propeller 

«endif 


0.313;  //  ft    Marco  -  4"    measured  -  3.75' 
0.313;  //  ft    Marco    4"    measured    3.75' 


// 


■// 
// 


Surge  equation  of  motion  coefficients 


const 

double 

X_u_dot 

const 

double 

X  V  dot 

const 

double 

X_w_dot 

const 

double 

X_p_dot 

const 

double 

X_q_dot 

•:cni:t 

double 

X_i_dot 

const 

double 

X  uu 

const 

doublf 

X  vv 

const 

double 

X  WW 

const 

double 

X^.p 

const 

double 

X_qq 

const 

double 

X_rr 

const 

double 

X_prop 

appli. 

rable 

const 

double 

X  rb 

const 

double 

X  rs 

■2.82E-3 
0.0 
0.0 
0.0 
0.0 
0.0 

0.0 

0.0 
0.0 
0.0 
0.0 
0.0 

7  .78E-3 


0.283 
-0.377 


Linear  force  coefficients  acting  in 
the  longitudinal  body  axis 
with  respect  to  subscripted 
motion  components 


// 
// 
// 
// 
// 
// 

// 

//  old  -1.743E-2   SDV-9  holdover? 

// 

// 

// 

//  old  -7.53E-3    SDV-9  holdover? 

//   X_prop  "constant"  no  longer 


L;  // 
L;  // 


cor.st  double 

const  double 

const  douL-le 

co.nst  double 

const  double 

const  douDle 

c"r."t  dcubie 

const  double 

const  double 

const  double 

const  double 

const  double 

coi;st  double 


X_u  u_de 1 1  a_b_de 1 1  a_D 
X_u  u_de 1 1  a_s_de 1 1  a_s 

X  uu  delta  r  delta  r 


X_pr 
X_wq 
X_vp 
X  vr 


0  .0 
0.0 
0.0 
0  .0 


X_u  q_de 1 1  a_bo w 
X_u q_de 1 t a_s t e r n 
X_ur_delta_rudder 
X_u  v_de 1 1  a_ru  dd  e  r 
X_uw_delta_bow 
X  uw  delta  stern 


•1  .018E-2 

■1 .018E-2 
■1 .018E-2 


//  drag  due  to  bow  plane 
//  drag  due  to  stern  plane 
//  drag  due  to  rudder 


// 
// 
// 
// 


0.0 
0.0 
0.0 
0.0 
0.0 
0.0 


;these  aren't  in  Bahrke  thesis  model 


// 
// 
// 
// 
// 
// 


const  double 

const  double 

const  double 

const  double 

const  double 


X_qdsn 
X_wdsn 
X  dsdsn 


0.0 
0.0 
0.0 


speed_per_rpm  =  2.0  /  700.0 
C  do     =      0.00778 


//  no  longer  used  in  new  model 

//  no  longer  used  in  new  model 

//  no  longer  used  in  new  model 

//  steady  state:   0.0028571429 

//  =  (2.0  feet/sec)  per  700  rpm 
// 


II- 
II 


■II 
II 


Sway  equation  of  motion  coefficients 


const  double  Y_u_dot 

const  double  Y_v_dot 

const  double  Y_w_dot 

const  double  Y_p_dot 

const  double  Y_q_dot 

const  double  Y  r  dot 


0.0 

// 

3 .43E-2 

// 

0.0 

// 

0.0 

// 

0.0 

// 

1 .78E-3 

// 

Linear  force  coefficients  acting  in 
the  athwartships  body  axis 
with  respect  to  subscripted 
motion  components 
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const  douhle 

cons!:  double 

const  double 

const  double 

const  double 

const  double 


Y_uu 
Y_uv 
Y_uw 
Y_up 
Y_uq 
Y  ur 


0.0 

-1 .07E-1 
0.0 
0  .0 
0.0 
0.0 


// 
// 
// 
// 
// 
// 


Warner  =  1.187E-2;   Bahrke 


0.0 


const  double  Y_uu_deita_rb  = 
const  double  Y  uu  delta  rs  = 


1.18E-2   ;  //  Marco  =  1.241E-2; 
1.18E-2   ;  //  Marco  =  1.241E-2; 


Bahrke  =  1 .18E-2 
Bahrke  =  1 . 18E-2 


const 

double 

Y_pq 

zz 

0 

0 

// 

(these  aren't  in  Bahrke  thesis  model) 

const 

double 

Y_qr 

- 

0 

0 

// 

const 

double 

Y_vq 

= 

0 

0 

// 

const 

double 

Y_wp 

= 

0 

0 

// 

const 

doutle 

Y  wr 

= 

0 

0 

// 

const 

double 

Y_-^ 

= 

0 

0 

// 

const 

double 

C_dy 

= 

0 

5 

// 

/ 

/  / 

~ 

~  '~ 

// 

// 

Heave  equa 

tion  of  motion  coefficients                         // 

con:?r 

double 

Z_u_dot 

= 

0 

0 

// 

Linear  force  coefficients  acting  in 

cons: 

double 

Z_v_dot 

= 

0 

0 

// 

the  vertical  bod>'  axis 

const 

dCuL'lc 

Z_w_dct 

= 

-9 

34E-2 

// 

with  respect  to  subscripted 

const 

double 

Z_p_dot 

= 

0 

0 

// 

motion  components 

const 

double 

Z_q_dot 

= 

-2 

52E-3 

// 

■Z  '-'-'Vt  -?  r 

:l'i^L  1  T 

Z_r_dct 

= 

G 

^ 

// 

const 

do- Die 

Z_v\- 

- 

0 

0 

// 

coi,st 

douii/ie 

Z  uw 

= 

-7 

644 E-1 

/  / 

const 

double 

Z_up 

= 

0 

0 

// 

const 

double 

Z_uq 

= 

0 

0 

// 

Marco  =  -7.013E-2;   Bahrke  =  0.0 

const 

double 

Z_rr 

= 

0 

0 

// 

const 

double 

Z_pp 

= 

0 

0 

// 

const 

double 

Z_uu_de. 

.ta 

_b 

=  . 

-  2. HE-: 

) 

// 

const 

double 

Z_uu_deita 

_s 

=  - 

-  2.  HE-: 

) 

// 

COI'.St 

double 

Z_pr 

- 

0 

0 

// 

(these  aren't  in  Bahrke  thesis  model) 

const 

double 

Z_vp 

= 

0 

0 

// 

const 

double 

Z_vr 

= 

0 

0 

// 

const 

double 

Z_qn 

= 

0 

0 

// 

no  longer  used  in  new  model 

const 

double 

Z_wn 

= 

0 

0 

// 

no  longer  used  in  new  model 

const 

double 

Z_dsn 

= 

0 

0 

// 

no  longer  used  in  new  model 

const  doub] 


C  d: 


0.6 


// 


// 

// 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 

const  double 


■// 

// 


Roll   equation  of  motion  coefficients 


K  u  dot 

= 

0.0 

// 

K  V  dot 

= 

0.0 

// 

K  w  dot 

= 

0.0 

// 

K_p_dot 

= 

-2  .4E-4 

// 

K_q_dot 

= 

0.0 

// 

K_r_dot 

= 

0.0 

// 

K  uu 

- 

0.0 

// 

K  uv 

= 

0.0 

// 

K  uw 

= 

0.0 

// 

K_up 

= 

-5.4E-3 

// 

K_uq 

= 

0.0 

// 

K  ur 

= 

0.0 

// 

Angular  force  coefficient  acting 
about  the  longitudinal  body  axis 
with  respect  to  subscripted 
motion  components 


//  surge-related  roll  damping  drag 


const  double   K_uu_planes  =  0.0 


;  //   (these  aren't  in  Bahrke  thesis  model 
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const 

double 

Kj^q 

const 

double 

K  qr 

const 

double 

K_vq 

const 

double 

K_wp 

const 

double 

K  wr 

const 

double 

K  \T«; 

const 

double 

K_prop 

const 

double 

K_pn 

const 

double 

K_PP 

const  double   K_p' 


0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

-2.02E-2 

// 

// 

// 

// 

K_pp/57.3 

•  // 

// 

K_prop  "constant"  no  longer  applicable 

no  longer  used  in  new  model 

test  value  for  p-squared  damping 

static  roll  damping  drag 
100  *  Healey  minimal  estimate 
(-2.02E-4) 

estimate  based  on  quadratic  term 
(K_pp)  equivalent  damping  at  1  deg/sec 


II- 

I ; 


■II 

II 


Pitch  e>quation  of  motion  coefficients 


const 

double 

M  u  dot 

rons  t 

double 

M_v_dot 

const 

double 

M  w  dot 

const 

double 

M_p_dot 

const 

double 

M_q_dot 

const 

double 

M_r_dot 

const 

double 

M  uu 

const 

double 

M  vv 

const 

double 

M  uw 

const 

double 

M_pp 

const 

double 

M_rr 

const 

double 

M_uq 

^-.nns: 

doL-ble 

M  uu  de 

0.0 

// 

C  .0 

// 

2.53E-3 

// 

C  .0 

// 

6.25E-3 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

0.0 

// 

Angular  force  coefficient  acting 
about  the  athwartships  body  axis 
with  respect  to  subscripted 
motion  components 


-1.53E-2   ;  //  surge-related  pitch  damping  drag 


^ta 


coi'iSt    double      K   uu    delta 


.bow 
/  / 
// 

.stern 
'// 
// 


-  0.283  *  L  *  Z_uu_delta_b; 

note  (-)  Z_uu_delta_b 

0 .043602433 

-f  0.3  77  *  L  '  Z_uu_delta_s; 

note  (-)  Z_uu_delta_s 

-  0.058085219 


const 

double 

M_pr 

const 

double 

K_vp 

const 

double 

K  vr 

const 

double 

M_j"jrop 

const 

double 

M_qn 

const 

double 

M  wn 

const 

double 

M_dsn 

const  double   M_qq 


const  double  M_q 


0.0 
0  .0 
0  .0 
0  .0 

0  .0 
0.0 
CO 


// 
// 
// 


;these  aren't  in  Bahrke  thesis  model; 


//  M_prop  "constant"  no  longer  applicable 

//  no  longer  used  in  new  model 
//no  longer  used  in  new  model 

//  no  longer  used  in  new  model 

-7.00E-3   ;  //  slightly  larger  than  N_rr  estimate 

//  test  value  for  q-squared 
//    static  pitch  damping  drag 

//  estimated  M_qq  ~  K_pp  *  length  /  width 

//  Torsiello   ~  0.005*  7.3'/  10.1"  =  .005 

=  M_qq  /  57.3;   //  estimate  based  on  quadratic  term 

//  (M_qq)  equivalent  damping  at  1  deg/sec 


II- 

II 


■II 
II 


Yaw   equation  of  motion  coefficients 
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ccni?r 

double 

N_u_dot 

= 

0 

0 

// 

cor.sc 

double 

N_v_dot 

z: 

-1 

78E-3 

// 

cor.:?: 

double 

N_w_dot 

= 

c 

0 

// 

con^r 

double 

N_p_dot 

= 

0 

0 

// 

COV-.S'^ 

double 

K_q_dot 

= 

0 

0 

// 

>.  -, .-  -.  ^ 

doubl- 

K_i_dr:-.c 

= 

-4 

^E-A 

/'    / 

const 

double 

N_uu 

= 

0 

0 

// 

const 

double 

N_uv 

= 

0 

0 

// 

co:.;;-t 

dOULl-:- 

N_uw 

= 

u 

0 

// 

const 

doutie 

N_up 

= 

0 

0 

// 

const 

double 

N_ug 

= 

0 

0 

// 

const 

double 

N_ur 

= 

-3 

90E-3 

// 

Angular  force  coefficient  acting 
about  the  vertical  body  axis 
with  respect  to  subscripted 
motion  components 


Marco 


•7 .69E-3 


surge-related  yaw  damping  drag 

/ /  N_uu_del ta_rb  and  N_uu_del ta_rs  not  symmetric  due  to  different  moment  arms 

//  const  double  N_uu_del ta_rb  -    1.3259  ;  //  Marco 

//  const  double  N_uu_delta_rs  =  1.7663  ;  //  Marco 

const  double  N_uu_del ta_rb  =   0.283  *  L  *  Y_uu_del ta_rb;  //  Bahrke   = 

0.02437762 

ccns:  double   N_uu_del ta_rs  =   0.377  »  L  '  Y_uu_del ta_rs;  //  Bahrke   = 

0.G324747G 


coYi-rt    douLltr   N_f.'rop   = 


0.0       ;  //  Normally  0.0  yaw  moment  due  to  paired 
//  counter-rotating  propellers; 
/ /  however  N_prop  is  not  zero  if  propeller  rpms  are  independent 
//  thus  yaw  equation  of  motion  now  has  yaw  moments  due  to  propellers 
//  and  K_prop  "constant"  is  no  longer  applicable 


cor 

St 

double 

N_pq 

ccr 

:  t 

aouce 

N   qr 

ccv 

sr 

dou  t;  J.  e 

N_vq 

C  CI 

St 

doubl t 

N   wt. 

co; 

St 

aout/ie 

N   wi 

cor 

St 

double 

N_v-^- 

0.0 

0.0 
0.0 
0.0 
0  .0 
0.0 


// 
// 
// 
// 
// 
// 


ithese  aren't  in  Bahrke  thesis  model 


con; 


OCl 


N  rr 


//  alternate  N_rr 
/  / 

const  double  N  r 


-5.48E-3   ;  //  Torsiello  value  p. 113  adjusted  for  L"B 
//  correction;   static  yaw  damping  drag 

N_rr  ~  M_qq  *  height/  width 
=  .040  ' 
=  0.005 
0.005 
N_rr  ~  M_qq 


//  estimated 

// 

// 

/  /' 


10.1"  /  16.5" 


Torsiello ; 


//  Healey: 

2  *  2*  *  1.92'  /(rho/2  L"5  *  r_max  *  r_max)  =>  0.0048473244 
using  r_max  =  16  deg/sec  Torsiello  which  is  consistent 

=   N_rr  /  57.3;   //  estimate  based  on  quadratic  term 

//  (N_rr)  equivalent  damping  at  1  deg/sec 


//  from  Dave  Marco's  dynamics  model: 

//  DEFINE  THE  LENGTH  X,  BREADTH  BR,  AND  HEIGHT  HH  TERMS 

double  XX  [15j  =  { 

-43.9/12.0, 

-39.2/12.0, 

-35.2/12.0, 

-31 .2/12.0, 

-27.2/12 .0, 

-10.0/12 .0, 

0.0/12.0, 

10.0/12 .0, 

26.8/12 .0, 

3  2  .  (J  /  1 2  .  0  , 


•// 
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37.8/12 
40.8/12 
4  2.3/12 
43.3/12 
4  3.7/12 


0, 
0, 
0, 
0, 
0} 


dcubie 
0.0/12 
2.7/12 
5.2/12 
7.6/12 

i/i: 


.0 


hh 
0, 
0, 
0, 
0, 
.0. 


[15]    = 


1 /12.C 
1/12. C 
1 ;  1 2  .  C 
1/12.C 

9.6/12.0, 
7.6/12.0, 
5  .  6  / 1 2  .  C' , 


2.3/12.0, 

■, .  .  u  .'  1 2  .  0  1 

doubie  bb 
16.5/12.: 
:  o  .  5  /  1 2  .  0 

.L  6  .  5  ■'  1 2  .  0 


16 

lb 


u 
0 

0 
0 

12.4-12.0 
9.5/12.0, 
7.0/12 .0, 
4.0/12.0, 
.J  .  U  /  1 2  .  0  ( 


-'  /  ^  z 
5/12 

5/12 
5/12 
5/12 


■// 


*tendi  f 


//    UUVMODEL    H 
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E.  UUVBody.C  Unmanned  Underwater  Vehicle  Networked  Rigid  Body 

:!;•;:.'.'  I  i :  I  ■'!!!  i  n  I !  1 1 1 1  : 1 1 1 1 1 !  I !  1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

Program:  UUVBody.C 

E)escripcion  :       Six  degree-of -freedom  underwater  vehicle  hydrodynairiics 
based  on  Healey  model 

Revised:  26  October  94 

System:  Irix  5.2 

Compiler:  ANSI  C++ 

Compilation:       irix>  make  dynamics 

irix>   CC  ULTVBody.C   -Im   -c    -g   +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 
+w  ==  Warn  about  all  questionable  constructs. 

Advisors:  Dr.  Mike  Zyda ,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Aut.-.or:  Don  Srutzmian  brutzman@cs.nps.navy.mil 

Code  OR /Br 

Naval  Postgraduate  School  (408)  656-2149  work 

Monterey  CA  93943-5000  (408)  656-2595  fax 

References:        Healey,  A.J.  and  Lienard,  D.,  "Multivariable  Sliding  Mode 
Control  for  Autonomous  Diving  and  Steering  of  Unmanned 
Underwater  Vehicles,"  IEEE  Journal  of  Oceanic  Engineering, 
vol.  18  no.  3,  July  1993,  pp.  327-339. 

Yuh,  J.,  "Modeling  and  Control  of  Underwater  Robotic 
Venicle, "  IEEE  Transactions  on  Systems,  Man  and  Cybernetics, 
vol.  20  no.  6.  Ncvemoer/December  1990,  pp.  1475-1483. 

Press,  William  H.,  Teukolsky,  Saul  A.,  Vetterling, 
William  T.  and  Flannery,  Brian  P.,  "Numerical  Recipes  in  C, " 
second  edition,  Cambridge  University  Press,  Cambridge 
England,  1992. 

Marco,  David.   "Slow  Speed  Control  and  Dynamic  Positioning 

of  an  Autonomcus  Vehicle,"  Ph.D.  dissertation. 

Naval  Postgraduate  School,  Monterey  California,  March  1995. 

Fossen,  Thor  I.,  _Guidance  and  Control  of  Ocean  Vehicles_, 
John  Wiley  and  Sons,  Chichester  England,  1994. 

Status:  Equations  of  motion  tested  satisfactorily, 

verification  against  in-water  tests  remains. 
Move  utilities  to  math_utilities . c 

Future  wcrk:       Comments  and  suggestions  are  welcome! 

*/ 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 j 1 1 1 1 1 1 1 1 1 1 

#ifndef  UUVBODY_C 

#define  UUVBODY_C    //  prevent  errors  if  multiple  ^includes  present 

char  *  DISCLIENT  =  "~/DIS .mcast /src/client  -r  & " ; 
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#incjud^  "SonarModel . C" 

# include  "UUVmodel.H" 

#includt  "DISNetworkedRigidBody  .C" 

1 1 1 1  n  i  1 1 1 1 1  / 1 1 1 1 1 1 11 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

class    ULTv^Body     :    public    DISNetworkedRigidBody 

private: 

Hmatrix  Hprevious  ; 

//  member  data  fields 

double     current_UL:v_t  ime; 


double  U 

doul:ie  V 

double  W 

dcui:lt-  P 

double  Q 

double  R 


double  u_dot 

double  v_dot 

double  w_dot 

dcuLle  P_dot 

double  i:j_dot 

double  r_dot 

douhle  x_dot 

douolt  y_dot 

double  z  dot 


double  phi_dot; 

double  theta_dot; 

double  psi_dct; 

doutjle  mass     I  6  )  16]; 

double  mass_inverse  [6j  [6], 

double  rhs  [6 ) ; 

double  new_acceleration  [5; 

double  new_velocity  | 6 ] ; 


//  Linear  &  angular  velocities 

//    in  body  coordinates 

//    (note:   RigidBody  is  world  coordinates 


//  Linear  &  angular  accelerations 
//    in  body  coordinates 


//  Euler  velocities,  world  coordinates 

//  Euler  rotation  rates,  world  coordinates 

//  mass  matrix  acceleration  coefficients 

/ /  mass  matrix  inverted 

//  right-hand-sides  of  equations  of  motion 

//  product  [mass_inverse] [rhs] 

//  (averaged_accelerations  *  dt) 

//  +  old  velocities 


//  AITV  telemetry  state  vector  is  located  in  "AUV_globals .  h"  file 
publ i  C : 
//  member  constructor  and  destructor  functions 


UUVBody 

-UUVBody 

//  operators 

void       mul tiply6x6_6 


0  ; 


0 


{  /*  null  body  •/  ) 


(double   left_matrix    [6] [6; 
double   right_matrix   [6], 
double   result_matrix  [6]); 
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void 


mul tiply6x6_6x6 


fraend  oscream^  operator  << 

inspection  methods 

void  princ_uuvbody 

void       print_matrix6 
void       print_mat rixbx6 

double      current  uuv  time  value 


double 

u_dot_value 

double 

v_dot_value 

double 

w_dot_value 

double 

p_dot_value 

double 

q_dot_value 

double 

r_dot_value 

double 

x_dot_value 

double 

y_dot_value 

douDle 

z_dot_val ue 

modi  ty  ma 

methods 

void 

•JWBod\_ini 

void 

set_carrent 

void 

set_acceler 

vol  a 


void 


void 


;et  linear  accelerations 


set  anaular  accelerations 


(double  left_matrix    [  6  "j  [  6  ; 

double  right_matrix   [61 [6; 

double  resulc_matrix  [6] [6; 

(ostream£(  out,  UUVBodyS.  uuvb)  , 


(double  output_matrix  [6]); 
(double  output_matrix  (6) [6] 

( )  const; 


const; 
const ; 
const; 
const ; 
const ; 
const; 


const ; 
const ; 


{ ;    const 


0  ; 


(double  new_current_uuv_tiroe) 

(double  new_u_dot, 
double  new_v_dot, 
double  new_w_dot, 
double  new_p_dot, 
double  new_q_dot, 
double   new_r_dot); 

(double  new_u_dot, 
double  new_v_dot, 
double   new_w_dot); 

(double  new_p_dot, 
double  new_q_dot, 
double   new   r   dot); 


/ /  vehicle  socket  communications  with  d^'namics 
loop_test_with_execution_level  ()  ; 

//  vehicle  socket  communications  with  no  dynamics 


void 

dead_reckon 

void 

swap 

double 

square 

double 

nonzerosign 

void 

clamp 

double     epsilon 

void       invert  mass  matrix 


(double  *  a,  double  *  b) ; 

(double  value) ; 

(double  value) ; 

(double  *  clampee, 

double  absolute_inin, 

double  absolute_max, 

char  *  name) ; 

0  ; 

0  ; 
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void 
void 
void 


cest:_invert_mat  rix  {) 

calculate_mass_inacrix  () 

integrate_equai:ions_of_motion       () 


); 


I  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 


, , 

//  constructor  method? 
/,' 


■// 
-// 


nu\'Body  :  :    UWEodv 


//   default    constructor 
Hpr^^vious    =   Hinatrix    { )  ; 


RiqidBody_in: t ialize  ( ) ; 
:'IFN*^''workedPigidBody_ini tialize  (  ) 


//  inherited  method 
//  inherited  method 


uuv  time  -    u .  I 


/  /- 


U 
V 

w 

p 

r 
R 

u_dor 
v_dc  t 
w_dot 

p_dot 
q_dot 
r_dot 

y._dot 

y_dot 

2_dot 

ph:_dot 

theta_dot 

i::ri_dot 


for  (int  index  =  0;  index  <=  5;  index++)  rhs  [index]  =  0.0; 

calculate_ma3s_matrix    {); 
i  r. ve  r  t _rria s  £_ma  t  r  1  >:  ( )  ; 


orjeratcr:- 


= 

Q 

0 

■z. 

0 

0 

- 

0 

0 

- 

0 

0 

- 

0 

0 

= 

0 

0 

- 

0 

0 

= 

0 

0 

= 

0 

0 

=: 

0 

0 

:; 

0 

0 

= 

c 

0 

_ 

0 

0 

= 

0 

0 

= 

0 

0 

= 

0 

0 

=; 

0 

0 

= 

0 

-; 

ostream^  operator  •'•:  (ostreami<  out,  UUVBodyi  uuvb) 
{ 

int  row,  col ; 

uuvb.print_networkedrigidbody  ()  ; 

out  <<  •current_uuv_time  =  "   <<  uuvb.current_uuv_time  <<  endl; 

out  <<  "<u_dot,  v_dot,  w_dot,  p_dot,  q_dot,  r_dot>  =  "  <<  •<" 

<<   uuvb.u_dot  <<  " ,  "  <<   uuvb.v_dot  <<  " ,  "  <<  uuvb.w_dot  <<  ", 
«   uuvb.p_dot  <<  " ,  "  <<  uuvb.q_dot  <<  " ,  "  <<  uuvb.r_dot  <<  ">' 
«  endl ; 

out   <<-  "Mass  matrix:"  <<  endl  ; 
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for  (row  =  0;  row  <=  5;  row  ++) 
( 

out  <•-  "  i  "  ; 

for  (col  =  0;  col  <=  5;  col  f+) 

( 

out  <<  uuvb.ir.ass  [row]  [col]; 
if    (col  <  5 )  out  <<","; 
else  out  <<  " ] "  <<  endl; 

} 
) 

out   -<  "Mass  inverse  matrix:"  <<   endl ; 
for  (row  =  0;  row  <=  5;  row  ++) 

out  <'  "  i  "  ; 

for  (col  =  0;  col  ■'=  5;  col  ++) 
( 

out  -•  uuvb. mass_inverse  [row] [col]; 

if    (col  <r    5i  out  <<  ",  "; 

else  out  «    "]"  <<  endl; 

} 

re  t  u  rn  (out) ; 

} 

// // 

void  UUVBody : :  mul t iply6x6_6  (double    left_matrix    [6] [6], 

double   right_matrix   [6], 
double   result_matrix  [6]) 

ir;t  row,  col  ; 

for  ;rcw  -   0;  row  <-   5;  row*+} 
i 

result_matrix  [row]  =  0.0; 

for  (col  =  0;  col  <-    5;  coI+t) 
! 

resi.lt_matrix  [row]  =  result_matrix  [row]  + 

lef t_iriatrix  [row]  [col]  *  right_matrix  [row]; 
} 

I 
\ 
// // 

vc:d  inj/Pcdy  :  :  mult  iply6x6_6x6  (double   lef  t_inatrix    [6]  [6], 

double   right_rnatrix   [6]  [6], 
double   result_inatrix  [6]  [6]) 

{ 

int  row,  col ,  index; 

for  (row  =  0;  row  <^  5;  row++) 
i 

for  (col  =  0;  col  <=  5;  col++) 

result_matrix  [row] [col]  =  0.0; 

for  (index  =  0;  index  <=  5;  index++) 
{ 

result_inatrix  [row]  [col]  =  result_matrix  [row]  [col]  + 

left_macrix  [row]  [index]  *  right_iTiatrix  [index]  [col]  ; 


// // 

//  ingpection  methods 
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// // 

void  UirvBody :  :    print_uuvbody    () 
i 

pi  inL_iie:workedrigidljody    (  )  ; 

cout  -  *  "cur rent_uuv_time   =  "   <<  current_uuv_time  <<  endl ; 

couc  «  "<U,  U,  W,  P,  Q,  R>  body  =  "  «  "<" 
<<  U  <<  " ,  "  <<  V  <<  " ,  "  <<  W  <<  " ,  " 
<<  P  <<  ",  "  <<  Q  <<  " ,  "  <<  R  <<  •>"  <<  endl ; 

couc  «  "<u_dot,  v_dot ,  w_dot,  p_dot ,  q_dot,  r_dot>  body  -    '    <<    '<" 
«   u_dot  <<  " ,  "  <<  v_doc  <<  " ,  "  <<  w_dot  <<  " ,  " 
<-^  p_dot  «  ",  "  <<  q_dot  <<  " ,  "  <<  r_dot  <<  ">"  <<  endl  ; 

VecrnrBE"  euier_po?i  t  ion_rates  =   Vector3D  {x_dot,  y_dot,  z_dot )  ; 

couc   <<-  "•-x_aot,    y_dot,      z_doc>   =  "  <<  euler_posi t ion_rates  <<  endl; 

Vecror3D  euler_angular_rates  =  VectorBD  (phi_dot,  theta_dot,  psi_dot); 
cout   --''  "•-phi_doc,  theta_dot,  psi_dot>  =  "  «  euler_angular_rates  «  endl; 

cour   '-'-  "Mass  matrix:"  -'-'  endl; 
pr  inr_rT'=ir  Mxbx^.  'rria?:?  ; 

cout   -'  "Kiss  inverse  matrix:"  «  endl; 
print_m5 t rixcxb  (mass_inverse) ; 

cout  <■<    "Right-hand  side  (RHSl  of  equations  of  motion: 
print_matrix6  (rns); 

;/ // 

void  LTL'^/Body :  :  prinC_matrix£  (double  output_matrix  [6]) 
( 

int  row; 

cout  ----  "  ;  "  ; 

for  ;  row  =  0;  row  <=  5;  row  ++) 

I 

cout  <-^  output_matrix  [row]; 

if  (row  <•  5)  cout  <<  ",  "; 
I 
cout  <''    "  1  "  •'•-  endl ; 

11 // 

void  ;JUVBody  :  :  print_matrix6x6  (double  output_matrix  [6]  [6]) 
1 

int  row,  col ; 

for  (row  =  0;  row  <-    5;  row  ++) 
I 

cout  <■■    "  [ " ; 

for  (col  =  0;  col  <=  5;  col  ++) 

{ 

cout  <^  <  output_matrix  [row]  [col]; 

i  f    (col  <  5)  cout  «  " ,  " ; 

else  cout  «  "]"  <<  endl; 

} 

) 

) 

// // 

double  UUVBody : :  current_uuv_time_value  () 

const 

{ 

return  current_uuv_time; 
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/  // 

double   ITTJVBody::    u_dot_value    {) 

consc 

\ 

return  u_dot ; 
} 
// // 

double  Uirv'Body :  :    v_dot_value    () 
cor.L-c 

return  v_dot ; 

; // 

dcuLle   'JiJVBody :  :    w_doc_value    () 

const 

i 

return   w_dot; 

} 

// // 

double  Ul.i'.'Eody::    p_dct_value    (i 
const 

return   p_dot ; 

'    // 

double  'J'.''^'5ody :  :    (3_dct_value    (; 

// // 

d c u  i:  1  e   ',".'3 c ay  :  :    r_do  t _\'a  1  u  e    i ) 

retur;;    r_dot  ; 
/    // 

double   UUVBody : :    x_dot_value    () 

i 

returr.   x_dot; 

) 

// // 

double  UUVBody::    y_dot_value    () 

const 

{ 

return  y_dot; 

} 

// // 

double  UU\'Body :  :    z_dot_value    () 

const 

{ 

return   z_dot ; 

) 

// // 

//    modifying  methods 

// // 
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void  Uin/Bcdy::    Uir\^Body_initialize    () 
( 

RigidEody_initialize    { ) ; 

DISNetworkedRigidBody_initialize    ( ) ; 


//    inherited  method 
//    inherited  method 


:urrent_DUv_cime   =    0.0; 


u 

;; 

0.0 

V 

= 

0.0 

w 

= 

0.0 

p 

= 

0.0 

Q 

= 

0.0 

R 

= 

0.0 

D   dot 

- 

0.0 

V  aor 

- 

0  .0 

W    dot 

= 

0.0 

p_dct 

= 

0.0 

CL_dot 

= 

0.0 

r_dot 

= 

0.0 

X   dot 

- 

0.0 

y_dot 

= 

0.0 

z_dot 

= 

0.0 

pr.:_dc 

^  ^ 

= 

CO 

theta_ 

dot 

= 

0.0 

p51_d0t 

= 

0.0 

for    1  j 

nt    index 

=    0 

;    in 

index++)    rhs    [index]    =    0.0; 


calculace_niass_matrix    () 
invert   iTia3s_matrix  i; 


AL%'_E:T1000_bearinq  =  0.0 
AlA'_ST1000_rangt  =0.0 
AUv    STIOOO    3tre:-:Qth=    0.0 


//  ST_1000  conical  pencil  beam  bearing 
//  ST_1000  conical  pencil  beam  range 
//  £T_1000  conical  pencil  beam  strength 


AU^_ST72  5_bearing  =0.0 
AU\'_£T7  2  5_range  =0.0 
AlA/'  ET72  5  strenqtn  =  0.0 


//  ST_725   1  X  24   sector  beam  bearing 
//  ST_7  2  5   1  X  24   sector  beami  range 
//  ST_725   1  X  24   sector  beam  strength 


AUV_bow_vertical  =  0.0 

AUV_stern_vertical  =  0.0 

ALw_bow_iateral  =  0.0 

AUV  stern  lateral  =0.0 


//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 


Airv'_delta_rudder  =0.0 

AUV_del ta_planes  =    0.0 

AUV_iJort_rpm  =0.0 

AUV_stbd_rpm  =    0.0 


//  positive  is  bow  rudder  to  starboard 

//  positive  is  bow  planes  to  starboard 

//  propeller  revolutions  per  minute 

//  propellor  revolutions  per  minute 


■// 


void  UUVBody : :  set_current_uuv_time  (double  new_current_uuv_time) 
i 

//- 


current_uuv_time  =  new_current_uuv_time; 


■// 


void  UUVBody::  set_accelerations 


(double  new_u_dot,  double  new_v_dot, 
double  new_w_dot ,  double  new_p_dot, 
double  new_q_dot,  double  new_r_dot; 


u_dot  =  new_u_dot 
v_dot  =  new_v_dot 
w  dot  =  new  w  dot 
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p_dot  =  new_p_dot; 
i_dcr  =  new_'.3_doC; 
i_doc    -    new_r_dot; 


void  ITlTVBodj' :  :    set_l  inear_accel  erac  ions      (double  new_u_dot,    double  new_v_dot, 

douDle  new_w_dGt) 

u_dot  =  new_u_dot:  ; 
v_doc  -  new_v_dor; 
w_dot:    =   new_v._dot; 


■// 


,  / // 

void  injVEody : :    set_angular_accelerations    (double  new_p_dot,    double  new_q_dot, 

double  new_r_dot) 

p_dot  =  new_p_dot; 
:2_dr)r  =  new_q_dot; 
i_doc    =    new_r_dot; 

;  // 

//    ve^.::ie    socKe:    corrjr.unications    resis 

/    // 

void  UlT/Body  :  :  loop_test_wi  th_execucion_level  () 

int  read_f rorT_socket_resulC  -   0; 
in:  writ€_tG_scckeL_result  =  0; 

/  /  pi -r.:_uuvbod>-  (  )  ;   //  diagnostic 

if  :TRACE;  roue  ^-<  "  iloop_test_witn_execution_level  scart)"  <<  endl; 

op'er._cxerut:i  on_level_sccket  [i;  /  /    repeated  calls  should  not  reopen  it 


ccut  <<  endl 

ccut  ■■<  "To 

• •  endl 

•:c-:  ■-<  DIE'rilErc:  --•  endl; 

cout  ' «-  endl 


isten  en  default  multicast  port  (on  another  machine; 


ccut  ■-■-    "I  I£_net_open  ():"  --<  endl  ; 
L:£_n.=  :_open  ,  ,.  ; 

while  (TRUE;   //  loop  until  Dreak 
i 

reaG_f  rorr,_socket_result  =  reaQ_f  rorri_execution_levei_socket  ( )  ; 

if  ( read_f roni_socket_result  ==  -4)    //  time/posi tion/orientation  received 
.  1 

cout  «  "position  or  orientation  command  received:   <" 

«  AUV_time  «  ">  <" 

«  AUV_x     «  " ,  "  <<  AUV_y      «    ' ,     '    «   AUV_z    «  " >   <  ° 

«  Airv'_phi   «  ',  "  <<  AUV_theta  <<  ",  "  <<  AUV_psi  «  ">" 
«  endl ; 

RigidBody_initialize  (); 

Vector3D  vectorl  =  Vector3D  (AUV_x,  AUV^ ,  AUV_z ) ; 

hmatrix  =  Hmatrix  (vectorl, 

radians  (AUV_phi ) , 

radians  (AUV_theta), 

radians  (AUV_psi ) ) ; 
hmatrix .print_hmatrix  ( ) ; 
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Hpievious  =  hmatrix; 
} 
else  if  i read_f rom_socket_resul t  ==  TRUE)  //  valid  report  received 

integrate_equations_of_motion     ( ) ; 

if  (DIS_net_write  ()  ==  FALSEj   break;  //  send  out  PDU,  otherwise  done 

test_tank_sonar_inodel  ( )  ;   //  parallelize  &  generalize  the  sonar  model 

write_to_execution_level_socket   ();      //  send  back  full  telemetry 
J 

if  ( (read_f rom_socket_result  --    -3)  II  //    KILL   signal  was  received 
f read_f rom_socket_resul t  ==  -4)   )  //  position  signal  was  received 

{ 

cout  <<  "KILL/position/orientation  signal  received,  "  <<  endl ; 
cout  <<  "freeze  the  AUV  where  it  is."  <<  endl ; 

//  surge  linear  velocity  along  x-axis 

//  sway  linear  velocity  along  y-axis 

//  heave  linear  velocity  along  x-axis 

//  roll  angular  velocity  about  x-axis 

//  pitch  angular  velocity  about  y-axis 

//  yaw  angular  velocity  about  z-axis 

//  linear  acceleration  along  x-axis 

//  linear  acceleration  along  y-axis 

//  linear  acceleration  along  x-axis 

//  angular  acceleration  about  x-axis 

//  •  angular  acceleration  about  y-axis 

//  angular  acceleration  about  z-axis 

//  Euler  velocity  along  North-axis 
//  Euler  velocity  along  East-axis 
//  Euler  velocity  along  Depth-axis 
//  Euler  rotation  rate  about  North-axis 
//  Euler  rotation  rate  about  East-axis 
//  Euler  rotation  rate  about  Depth-axis 

clock_t  start_busywai t_clock  =  clock  ( )  ; 

while   (clock  {)  <  start_busywait_clock  +  1.0  *  CLOCKS_PER_SEC) 

{ 

/*  Dusy  wait  •/ 
} 

wri te_to_socket_result  =  DIS_net_write  ( ) ; 
cout  -'.-  "freeze  AlW  PDU  sent:   "  <<   wri  te_to_socket_resul  t  «  endl; 


A'u^^f_U 

= 

0 

0 

AUV    v 

= 

0 

0 

AW   w 

= 

0 

0 

AU-_p 

= 

0 

0 

AUV_q 

= 

0 

0 

AU\'_r 

= 

0 

0 

AlT'\/_u_dot 

- 

0 

0 

A'Jv_v_doc 

= 

u 

0 

AUV_w_dot 

= 

0 

0 

AUV_iJ_dot 

= 

0 

0 

AUV_c^_dot 

=r 

0 

Ab'V_r_dot 

= 

0 

0 

AUV_x_dot 

- 

0 

0 

AUV_y    dot 

= 

0 

0 

AU\'_z_dot 

= 

0 

0 

A'L.T\''_phi_dot 

= 

0 

0 

AUV_theta_dot 

^ 

0 

0 

AU\-'_psi_dot 

= 

c 

0 

if  (read_f  ron"i_socket_result  =: 
{ 


•3) 


//  KILL  signal  was  received 


cout  <<  "KILL  signal  received,  exit  " 

<<  "'JUVBody  .  loop_test_with_execution_level "  «  endl; 

if  (TRACE)  cout  <<    " [ loop_test_with_execution_level  complete]" 
«   endl ; 

return;  //  all  done 

) 

//  unreachable  exit  point 

//  shutdown_socket  {)  must  have  been  invoked  already  to  exit  preceding  loop 

1   //  loop_test_wi th_execution_level  complete 

// // 


void  UUVBody : :  dead_reckon_test_wi th_execution_level  () 
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<<   endl 


mt    read_f  roir._socket_resul  t    =    0; 
inc    wri tt_to_sockec_resul t      =    0; 

rout  <  <  endl ; 

co^t    -•  "To  listen  on  default  multicast  port: 

cout  -.-.  LISCLIENT  <-'  endl  ; 

cout  -.-^  endl  ; 

open_execution_level_socket  ( ) ; 

cout  •-  "DIS_net_open  ()"  <<  endl ; 
DI£_net_open  ( j ; 

vw-hiie  (TRuE'   //  loop  until  break 

read_f  roiTi_socket_resul t  =  read_f  rom_execution_level_socket  (  )  ; 

if  ( { read_f rom_socket_result  ==  -2)  II  //  shutdown  signal  was  received 
(  r.rad_f  ioiri_socket_resul  t  =~    -3])         //        quit    signal  was  received 

{ 

cout  <■'  "shutdown  signal  received,  freeze  the  AUV  where  it  is" 
•-•  endi; 

//  surge  linear  velocity  along  x-axis 
//  sway  linear  velocity  along  y-axis 
//  heave  linear  velocity  along  x-axis 
//  roll  angular  velocity  about  x-axis 
//  pitch  angular  velocity  about  y-axis 
//  yaw   angular  velocity  about  z-axis 

//  linear  acceleration  along  x-axis 

//  linear  acceleration  along  y-axis 

//  linear  acceleration  along  x-axis 

//  angular  acceleration  about  x-axis 

//  angular  acceleration  about  y-axis 

//  angular  acceleration  about  z-axis 

//  Euler  velocity  along  North-axis 
//  Euler  velocity  along  East-axis 
//      Euler  velocity  along  Depth-axis 


AIA'    u 

= 

0 

0 

AUV_v 

= 

0 

0 

AUV_w 

= 

0 

0 

AUV_p 

= 

0 

0 

AUV_q 

= 

0 

0 

AUV_r 

= 

0 

0 

Airv_u_dot 

= 

0 

0 

AUV_v_dot 

= 

0 

0 

A'JV'_w_dot 

= 

0 

0 

A'J\'_p_30'i 

= 

0 

0 

A'Jv_q_dct 

= 

0 

0 

AU\'_r_dot 

= 

0 

0 

Ala'_x_dot 

= 

0 

0 

AUV'_y_dot 

= 

0 

0 

AUV_z_dot 

= 

0 

0 

AUV'_phi_dot 

= 

0 

0 

AU"v_theta_dot 

= 

0 

0 

AUV_psi_dot 

= 

0 

0 

II  Euler  rotation  rate  about  North-axis 
//  Euler  rotation  rate  about  East-axis 
//  Euler  rotation  rate  about  Depth-axis 


clock_t  start_busywai t_clock  =  clock  {), 
while   iclock  ()  <  start_busywai t_clock 


+1.0*  CLOCKS  PER  SEC) 


/'  \j\J.S\ 


wait 


wri te_t o_socket_resul t  =  DIS_net_wri te  ( ) ; 

cout    <■     "freeze  AUV   PDU   sent:       "    <<   write_to_socket_resul t    «   endl ; 


if    (read_f  roiTi_socket_result 


■3)  //  quit  signal  was  received 


cout  <<  "quit  signal  received,  freeze  the  AUV  where  it  is"  <<  endl ; 
cout  <<  "   and  rezero  for  another  loop  if  necessary"  <<  endl ; 
UUVBody_ini  tiali  ze  (  )  ; 
) 

break; 
) 

//  equations  of  motion  not  integrated,  execution  level  does  dead  reckoning 
if  (DIS_net_write  ()  ==  FALSE)    break;   //  send  out  a  PDU,  otherwise  done 


185 


wrice_to_execut ion_level_socket   ();      //  note  state  vector  preserved 
) 
//  shutdown_socket  ()  must  have  been  invoked  already  to  exit  preceding  loop 

//  cout  <-^    "DIS_net_close  {)"  «   endl  ; 
//  DIS_net_close  ( ) ; 

// // 

//  vehicle  dynamics  functions 

/y // 

void  UUVBody : :  swap  (double  *  a,  double  *  b) 
( 

double  temp  =  *  a; 

*  a  =  '  b; 

•*  b  =  t  emp  ; 

) 

// // 

double  'Ji.lVEody ;  :  square  (double  value) 

f 

return  'value  *  valued; 

} 

/  // 

double  injv'Body :  :  nonzerosign  idouble  value) 

if  lvalue  !=  0.0)  return   sign  (value); 
el se  return      +1.0  ; 


•// 


void  Ul.'VBody::  clamp  (double  *  clampee,  double  absolute_min, 

double  absolute_max,  char  *  name) 

double  new_value,  local_min,  locai_max; 

:f    :  ;absclute_max   ==    0.0)    s<6<    (ab3olute_min   ==   0.0))    return;      //   no   clamp 

if  (absolute_max  >=  absolute_min )  //  ensure  min  u   max  used  in  proper  order 
i 

local_min  =  absolute_min; 

local_max  =  absolute_max; 
) 

else 
( 

locai_min  =  absolute_max; 

local_max  =  absolute_min; 
1 

if  ( ' *  clampee)  >  local_max) 
( 

new_value  -    local_max; 

cout  •-•.  "clamping  "  «  name  «  "  from  " 

<<  *  clampee  «  "  to  "  «  new_value  <<  endl; 

*  clampee  =  new_value; 
) 

if  ((*  clampee)  <  local_min) 
{ 

new_value  =  local_min; 

cout  •■■:  "clamping  "  «  name  «  "  from  " 

■:•-  *  clampee  «  °  to  "  «  new_value  <<  endl; 

*  clampee  =  new_value; 
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* 


1 

;/ // 

douDle  UlTVBody :  :  epsilon  ()   //  not  used  in  revised  surge  EOM  ««««<<<<««< 

//  due  to  problems  in  analytic  derivation 
//  retained  for  archival  purposes 

Joucie        aver3qe_rpm  =  (AU\/'_port_rpm  +  AUV_stbd_rpin)  /  2.0; 

double  n    =  average_rpm; 

double  eta; 

>^Z'^    -    rpeed_pei_rpn'  *  n  /  nonzerosigr-  !  U  ); 

'  ■■  ccut  ----  "era     -    "    ■■■   eta  •'■-  endl  ; 

double  C_t   =  0.008  *  L'L  *  eta  '  fabs  (eta)  /  2.0; 

double  C_tl  =  0.008  *  L*L  /  2.0; 

i-ubl-^  re-ult; 

'■  .X_pro!'  redefinition:  net  pushing  force  on  vehicle,  account  for  water  flow 
••  X_pi'oi:  =  C_dG  '  (eta  '  fabs  'etai  -  1.0^; 
//  X_prop  =  C_dO  *  (eta  '  fabs  'eta;;; 
.'/  ecu:  ••  "X_prcp  =  "  <•-  X_prop  '-<  endl; 

result  =   -1.0  +  (Sign  (n)  /  nonzerosign  (  U  )) 

*  isqrt  (C_t  -    1.0)  -  1.0)  /  (sqrt  (C_tl  ■>■    1.0)  -  1.0); 

//  kill  this  function  due  to  rewritten  X_propulsion  terms 

//  cout  ■  •'  "epsilon  ()  =  "  «  result  <<  endl; 

y^rtur:-.  result; 
// 

void  'JJ.'Scdy::    invert_m3;SS_rT.atrix    i)         //   adapted   frorri  Numierical    Recipes    in  C 

//  2nd  edition  pp.  3  9-40 

^r,t  indxc  16;,  indy.i  [6],  ipiv  16); 

^r.t  :  ,  :  ,  k,  row,  col  ,  1 ,  11  ; 

double  big,  dummy,  pivmv; 

double  ma3s_:oi:y  ;5][6;; 

for  (i  =  0;  i  <=  5;  i++) 

for  i]  =  0;  j  <=  5;  j++) 
( 

mass_copy  li][j]  =  mass  [i][j]; 
if     (i  ==  jy  mass_inver3e  [i][j]  =  1.0; 
else  mass_inverse  li]!j]  -   0.0; 

} 
) 
//  int  arrays  ipiv,  indxr  and  indxc  are  used  for  bookkeeping  on  the  pivoting 

for  (j  -    0;  j  <=  5;  j++)  ipiv  [j]  =  0;    //  initialize  loop 

for  (i  =  0;  i  <=  5;  1++)  //  main  loop  over  columns  reduced 

i 

big  =  0.0; 

for  (j  =  0;  j  <=  5;  j++)  //  outer  loop  of  pivot  search 

( 
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if  (ipiv  1] 


for  ik  =  0;  k  <=  5;  k-^*} 
{ 

if  (ipiv  [k]  ==  0) 

{ 


if  (fabs  (mass_copy  [j]lkl)  >=  big) 
( 

big  =  fabs  (inass_copy  [j](k]); 
row  =  j ; 
col  -  k; 
) 
) 
else  if  (ipiv  [ k 1  >  1 ) 

cout  <<  "Error:   singular  mass  matrix"  «  endl ; 


+  -^  ( Ipiv  [  col  1  )  ; 

//  see  detailed  comments  in  reference  code  for  pivot  bookkeeping  scheme 

i  f  ( row  I =  col ) 
( 

for  (1  =  0;  1  <=  5;  1++)  swap  (&  mass_copy     [row][l], 

&  mass_copy     [col][l]); 

for  (1  =  0;  1  <-  5;  1++)  swap  (&  mass_inverse  [row][l], 

&  mass_inverse  [col] [1] ) ; 
) 

indxr  [i]  =  row;  //we  are  now  ready  to  divide  pivot  row  by  pivot  element 
indxr  ^i)  =  col;  //    which  is  located  at  [row,  col] 

^t  (n'iass_copy  icolj[col]  ==  O.U) 

cout  <<  "invert  error,  singular  matrix"  «   endl; 

pivinv  =  1.0  /  mass_copy  [col] [col]; 
n.as£_copy  [col;  [col]  =  1.0; 


fori.   =  0 ;  1 
for  (1   =  0;  1 


5;   1- 
5;   1- 


for  (11  =  0;  11  <=  5;  11- 


if  (11  !=  col ) 
{ 

dummy  =  mass_copy  [11] [col]; 

mass_copy  [11] [col]  =  0.0; 

for  (1  =  0;  1  <=  5;  1+*) 

mass_copy     [ 11 ] [ 1 


mass_copy     [col][l]  *=  pivinv; 
mass_inverse  [col][l]  *=  pivinv; 

//  next  we  reduce  the  rows 

//  except  for  the  pivot  row,  of  course 


mass_copy 


[  c  o 1 ]  [ 1 ]  *  dummy ; 


for  (1  =  0;  1  <=  5;  1++) 

mass_inverse  [11] [1]  --   mass_inverse  [col][l]  *  dummy; 


//  we  now  unscramble  the  columns  by  interchanging  in  reverse  order 

for  (1  =  5;  1  >=  0;  1--) 
1 

if  (indxr  [1]  !=  indxc  [1 ] ) 
{ 

for  (k  =  0;  k  <-    5;  k++) 
{ 

swap    (&   mass_copy    [k] [indxr    [1]],    &   mass_copy    [k] [indxc    [1]]); 
) 
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//■ 


■// 


void  UUVBody 
i 

i  n  c    1  ,    j  ; 


test_invert_iTiatrix    () 


ca  J  Cu^  aLf_rria3S_ma t  ri X    {  )  ; 

cout    -.'^    "Original    mass   matrix:"    <<   endl; 

pr  int_rr:at:rix6x6     (iTiass,; 


invert   mass   matrix 


()  ; 


cout  •  ■  endl ; 

cout  ••  "Inverted  ma.??  matrix:"  --<  endl; 

pr int_matrix6xfe  !ma3-=_inverse)  ; 

for   I  -  0;  i  ■  =  5;  i--; 
{ 

for  (j  =  0;  1  <=  5;  j++) 

.Tias;-    [-;[];    =   iTiass_inverse    i  i  1  !  j  I  ; 


ii.vt^rt   mid  S3   iTiatrix 


()  ; 


CGut  ■  •  e;";a-  ; 

cout  <-<■  "Double  invert_mass_matrix  ()  should  get  back  to 
'  «-■  "original  mass  matrix:"  <<  endl  ; 

print_m,atr  1x6x6  (mass_inverse)  ; 

calculate_miass_matrix  ();   //  restore 
invert_mia3£_matrix     i); 


•// 


i;U'-'r-c,dy  ; 


calculate  mass  miatrix 


*def i  ne 

UDOT 

0 

Slitl^ntr 

VIiOT 

a 

»define 

WLOT 

2 

*def ine 

FEOT 

3 

«fdef  int^ 

OE'CT 

4 

"df-t":  n^ 

r  I  .'T 

r 

*def in^ 

SUFGE 

(.1 

•define 

SWAY 

1 

« define 

HEAVE 

2 

# define 

ROLL 

3 

»def ine 

PITCH 

4 

*def ine 

YAW 

5 

mass  [SURGE] [UDOT] 

mass  [SURGE] [VDOT] 

mass  [SURGE] [WDOT] 

mass  [SURGE] [PDOT] 

mass  [SURGE] [QDOT] 

mass  [SURGE] [ROOT] 


mass  [SWAY 

mass  [SWAY 

mass  [SWAY 

mass  [SWAY 

mass  [SWAY 

mass  [SWAY 


[UDOT] 
[VDOT] 
[WDOT] 
[PDOT] 
[QDOT] 
[ROOT] 


//  matrix  indices 


m 

0.0 
0.0 
0.0 
m  * 
m  * 


-0.5 


rho 


L'L*L 


X  u  dot 


(  z_G) 

(-y_G) 


mass  [HEAVE] [UDOT] 


0.0; 

m 

0.0; 

m   *    (-z_G) 

0.0; 

m  •  (  x_G) 

0.0; 


0.5  '  rho  *  L*L*L 
0.5  *  rho  *  L*L*L*L 
0.5  •  rho  •  L*L*L*L 


*  Y_v_dot 

*  Y_p_dot 

*  Y  r  dot 
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mass 

[HEAVE] [VDOT] 

= 

0.0; 

mass 

[HEAVE] [WDOT] 

- 

m 

mass 

1  HEAVE]  [PDOTl 

= 

m  *  ( 

y. 

G) 

mass 

[HEAVE] [QDOT] 

= 

m  *  ' 

-X. 

-G) 

mas? 

[HEAVE] [RDOT] 

= 

0  .  0  ; 

mass 

[ROLL  ] [UDOT] 

= 

0.0; 

mass 

[ROLL  1 [VDOT] 

= 

m  *  ( 

-z. 

_G) 

mass 

[ROLL  ] [WDOT] 

= 

m  *  ( 

y. 

_G) 

mass 

[ROLL  ] [PDOT] 

= 

I_x 

mass 

[ROLL  ] [QDOT] 

= 

- i_xy ; 

mass 

[ROLL  ] [RDOT] 

= 

-I_xz 

mass 

[PITCH] [UDOT] 

= 

m  *  ( 

2. 

_G) 

mass 

[PITCH] [VDOT] 

- 

0.0; 

mass 

[PITCH] [WDOT] 

= 

m  *  { 

-X. 

_G) 

mass 

[PITCH] [PDOT] 

IT 

- I_xy ; 

mass 

[PITCH] [QDOT] 

= 

I^ 

mass 

[PITCH] [RDOT] 

= 

-I_yz; 

mass 

[YAW   ] [UDOT] 

= 

m  *  ' 

-y. 

_G' 

mass 

[YAW   ! [VDOT] 

= 

m  *  . 

X. 

_G) 

rr.a:5s 

[YAW   j  [WDOT; 

= 

CO; 

rnass 

:YAW   1 [PDOT] 

= 

-I  xz 

mass 

[YAW   ] [QDOT] 

- 

-i_yz; 

mass 

[YAW   ; [RDOT] 

= 

I  2 

0.5  *  rho   *  L*L*L  *    Z_w_doc , 

0.5  *  rho    *  L*L*L*L  *    Z_q_dot , 

0.5  •  rho    *  L*L*L*L  *    K_v_dot 

0.5  *  rho    *  L*L''L*L*L  *    K_p_dot 

0.5  *  rho    *  L*L*L*L*L  *    K_r_dot 

0.5  *  rho    *  L*L*L*L  *    M_w_dot , 

0.5  •  rho    *  L^L'L'-L^L  *    M_q_dot, 

0.5  *  rho    *  L*L*L*L  *    N_v_dot 

0.5  *  rho    *  L*L*L'-L*L  *    N_p_dot 

0.5  *  rho   *  L*L*L*L*L  *   N  r  dot 


/, 


■// 


'-•■",  j   -.n.'^'Pr.  dy  :  :    int:tgr3te_equa:  j  ons_of_mot:ion    () 
{ 

currer.t_uuv_cime  =  AU'v'_time; 

dr-utle  d:        =  current_uuv_tim6  -  time_of_posture_value  (); 

if  'dr  ■■  0.0!     //  missicr.  cloc)<  was  reset,  re2erc  the  dynamics  model 

•:urrer;t_uuv_t  ime    =  AlTV_c:me; 
set_time_of_posture  (AUV_time) ; 

set_veiccities  (0.0,  0.0,  0.0,  0.0,  0.0,  0.0); 
set_accelerations  (0.0,  0.0,  0.0,  0.0,  0.0,  0.0); 
dt  =  0.0, 


u 

^0.0 

V 

=  0.0 

w 

=  0.0 

p 

=  0.0 

Q 

=  0.0 

R 

=    0.0 

double 

rho2 

-   rho 

/ 

2 

.0; 

double 

L2 

=  L  * 

L 

double 

L3 

=  L  ' 

L 

* 

L; 

double 

L4 

=  L  * 

L 

* 

L  * 

L 

double 

L5 

=  L  • 

L 

* 

L  * 

L 

//  note  that  sign  is  not  preserved  in  the  following  squared  variables 
//      in  order  to  present  consistent  naming  with  Healey  reference  paper. 
//      To  preserve  sign,  use  (U  *  fabs  (U) )  etc. 
double  P2         =  F  *  P; 


double  Q2 

.  Q  * 

Q 

double  R2 

=  R  * 

R 

double  U2 

=  U  * 

U 

double  V2 

=  V  * 

V 

double  W2 

=  W  * 

W 
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//  calculace  world  coordinate  posture  rates,  use  holding  variables  for  speed 

double  PHI 
double  THETA 
double  P£I 

double  sinPHI 
double  cosPHI 
double  sinTHETA 
dcuLl^  cosTHETA 
iojble  sinPSI 
douole  cosPSI 

double  EPSILON 

dou'C'le   del  ta_[:ilanes_stern 
doui:.-e    del  raii-lanes    bow 


= 

phi_value 

0 

= 

theta_value 

{  ) 

= 

psi_value 

(  ) 

- 

sin 

PHI 

= 

cos 

PHI 

)  ; 

:= 

sin 

THETA 

)  ; 

IT 

cos 

THETA 

)  ; 

= 

sin 

P£I 

}  ; 

= 

cos 

PSI 

)  '■ 

= 

epsil 

on  (  )  ; 

/  / 

;    //    no   longer   used   in   revised  model 

AU\'_delta_p lanes  ; 
-    AlJV_delta_planes  ; 


double  del  ta_rudder_stern  -        AU\-'_delca_rudder; 
douLle  del ta_rudder_bow   =  -  AUV_delta_rudder ; 

//  int^qrite  drag  foices  over  the  vehicle  -  -  - 


double   3way_integral  =  0.0 

double  heave_integral  -  0.0 

dcuLlf  pitcJ'._integral  =  0.0 

dcuLle   yaw_integral  =  0.0 

doui,l.r  dx,  'J_cf_>:; 

for  .int  x_inde>:  =  0;  x_index  <  14;  x_index  ++)  //  longitudinal  center  line 
( 

dx  =  fabs  :xx  iX_indexI  -  xx  ix_index  -^  1]); 

U_cf_x  =  sqrt  i   square  (V  +  xx  [x_index]  *  R) 

+  square  (W  -  xx  lx_index]  *  Q) ) ; 

if  ib'_cf_x  •  i.OE-6)  //  arbitrary  small  non-0  minimum 

sway_integral   +=    rho2  *  {    C_dy  *  hh  [x_index] 

'  square  ( (V  +  xx  [x_indexl  *  R)) 

*  C_dz  *  bb  [x_index] 

*  square  ( (W  -  xx  (x_index]  *  Q) ) ) 

•  (V  +  XX  (x_index]  *  R)  *  dx  /  U_cf_x; 


heave  inteqral  += 


rho2  •  (  C_dy  •  hh  (x_index] 

*  square  ( (V  +  xx  [x_index]  *  R)) 
+  C_dz  *  bb  [x_index) 

*  square  ( (W  -  xx  [x_index]  •  Q) ) ) 
*  {W  -  XX  [x_index]  *  Q)  *  dx  /  U_cf_x; 


pitch_integral  += 


rho2  *  (  C_dy  *  hh  [x_index] 

•  square  ( {V  +  xx  [x_index]  *  R) 
+  C  dz  *  bb  [x  index] 


191 


*  square    ( (W   -   xx    [x_index]    *   Q) ) ) 

*  (W   -    x>:    [x_indexl    *    Q) 

//  '■  note  sign  correction 

*  XX  [x_index)  *  dx  /  U_cf_x; 

yaw_integral  +=   rho2  *  (  C_dy  *  hh  [x_indexl 

*  sg-aare  (  (V  +  xx  [x_index]  *  R)  ) 
+  C_dz  *  bb  [x_index] 

*  square  ( (W  -  xx  [x_index]  *  Q) ) ) 

*  (V  +  XX  [x_index]  *  R) 

*  XX  lx_index]  »  dx  /  U_cf_x; 


it     : TRACE) 


cout  << 
<< 


cout  << 


cout  << 


cout 


dx  =  "  <<  dx  <<  " ,  U_cf_x  =  "  <<  U_cf_x 

,  sway_integral  -  "  <<  sway_integral  «  endl ; 

dx  =  "  <<  dx  <<  ",  U_cf_x  =  "  «  U_cf_x 

,  heave_integral  =  "  <<   heave_integral  <<  endl ; 

d>:  =  "  <<  dx  <<  ",  U_cf_x  =  "  <<  U_cf_x 

,  pi tch_integrai  =  "  <<  pi tch_integral  <<  endl ; 

dx  =  "  <--<■  dx  <•--  ",  U_cf_x  =  "  «  U_cf_x 

,  yaw_integral    =  "  <-<  yaw_integral  <<   endl; 


i  1 1  '::'  i  1 1 1  1 1  /  I  !! n  !  I  1 1 1 1  !  I  1 1  1 11 1  1 1 n 1 1 1 1 1 1  1 1 1! 1 1 1 1  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 ! I 
i  I    calculate  Equations  of  Motion  riqht-hand  sides  // 

;!  i  1 1 1 1  i  n  i  1 1  n  1 1 1 1 1 1 11 1 !  n  1 11 1 1 : 1  i'l  1 1 1 1 1 1 11 1 1 1 1  n  1 1  n  1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 


rns  [SURGE]  =  //  Surge  Motion  Equation  rignt  hand  side  

n.  *  ((V  *  R)  -  (W  *  Q)  +  x_G  *  (02  +  R2 )  -  y_G  *  P*Q  -  z_G  '  P*R) 

+  rho2  *  L4  '  (   X_pp  *  P2   +  X_qq  *  Q2 

+  X_rr  *  R2   +  X_pr  *  P*R) 

+  rho2  '  L3  -  (   X_wq  •  W*Q  -  X_vp  *  V*P  +  X_vr  *  V*R 

+  U*Q  *  (   X_uq_del ta_bow  *  del ta_planes_bow 

+  X_uq_del ta_stern  *  del ta_planes_stern) 


+  U*R  *  (   X_ur_del ta_rudder   *  del ta_rudder_bow 

+  X_ur_delta_rudder   *  delta_rudder_stern) 
) 

+  rho2  *  L2  *  (   X_w  ♦  V2   +  X_ww  *  W2 

+  U*V  *  (   X_uv_delta_rudder   *  delta_rudder_stern) 

+  U*W  *  (   X_uw_delta_bow     *  delta_planes_bow 

+  X_uw_del ta_stern    *  del ta_planes_stern) 

+   U   *    fabs    (U)       •    (      X_uu_delta_b_delta_b 

*    delta_planes_bow 


•// 
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*  delca_planes_bow 

+  X_uu_delta_s_delta_s 
'  delta_pianes_stern 

*  delta_planes_stern 

+  X_uu_delta_r_delta_r 

*  delta_rudder_bow 

*  delta_rudder_bow 

+  X_uu_delta_r_delta_r 

*  delca_rudder_stern 

*  delta_rudder_stern) 
) 

-  I'Weigl-.t  -  Buoyancy'  *  smTHETA 

//  EPEILON  Lt-rm?  have  been  removed  due  to  revised  equations  of  motion 

•"  +  rho2  '  l;  '  X_qd?n  *  U*C  *  del  ta_planes_stern  *  EPEILON 
,  .  -  rhcZ  *  L2  *  EPSILOi:  '  v  X_wd3n  '  U*W  *  del ta_planes_stern 
// 

+  X_dsdsn  *  U2       *  del ta_planes_stern 

*  del ta_planes_stern) 

/,  X_propuision  surge  force  {derived  using  expressions  in  Healey  paper) 
note  tJiat  speed_per_rpm  is  associated  with  work  of  two  propellors 

'  rho2  '  12  *  C_dO  *   square  ( speed_per_rpm) 

0.5  *  (   AUV_port_rpm  "  fabs  (Airv'_port_rpm) 

+  AU\'_stbd_rpm  •  fabs  (AUV_stbd_rpm)  ) 

/ /■  X_resi3tance  surge  drag  (derived  using  expressions  in  Healey  paper! 

-  rho2  *  L2  »  C_dC  *  U  '  fabs  (U) ; 

^  /•  /  /•  /  /  i  I  i  I  n;  I  a  i  I !  1 1 : 1 1  i !:;  I !  i:  1 11 1 1 1 1 : 1 !  1 1 1 1 1 1 1  i  1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

~?J-.  JZ  Surge  TRACE 

cc-r  ■•  "  ■'    surg^  teiTrl="  •-•:   m  '  i(V  "  Ri  -  (W  '  Q) 

'0  -  z_G  •  P*R)«  endl; 

ccut  ■■  "tenT,2="  <<        *    rho2  '  L4  *  (   X_pp  ■*  P2   +  X_qq  *  Q2 

X_pr  *  P'R) 


''    s  u  r  ^'  cr 

teiTT 

1  = 

"     •-  •: 

m    ' 

-    x_g' ' 

iC2 

t 

R2)    - 

y_G 

"  tenT,2=  " 

<< 

+ 

rho2 

'    L4 

+ 

X_rr 

'    R2 

tr  r.  ^  i  ; 

"  t  e  iTTi  3  =  " 

<•• 

4 

rho2 

*    L3 

+ 

U'Q 

'     ( 

cout  ■"--    ^61-1713="  <••    4  rho2  *  L3  *  (   X_wq  *  W*Q  +  X_vp  *  V*P  4  X_vr  *  V*R 

X_uq_del ta_bow  *  del ta_planes_bow 

+  X_uq_del ta_stern  *  del ta_planes_stern) 

+  U*R  *  (   X_ur_delta_r udder  *  delta_rudder_stern 

4  X_ur_delta_rudder  *  del ta_rudder_bow) 
) 
<<  endl ; 

cout  «  "term4=''  «    +  rho2  *  L2  *  (   X_w  *  V2   +  X_ww  *  W2 

+  U*V  *  (   X_uv_del ta_rudder  *  delta_rudder_stern) 

4  U*W  *  (   X_uw_del ta_bow  *  del ta_p lane s_bow 

4  X_uw_delta_stern  *  delta_planes_stern) 

4  U  *  fabs  (U)   *  (   X_uu_delta_b_delta_b 
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*  delta_planes_bow 

*  delta_planes_bow 

+  X_uu_del ta_s_del ta_s 

*  delta_pianes_stern 

*  delta_planes_stern 

+  X_uu_delta_r_del ta_r 

*  delta_rudder_bow 

*  delta_rudder_bow 

+   X_uu_delta_r_del ta_r 

*  del ta_rudder_s tern 

*  delta  rudder  stern) 


indl  ; 


) 


cout    <-<    "tenT:5="    <<-    -     (Weight    -    Buoyancy)    *    smTHETA 
<-<    endl  ; 

cout    <'<    "  termo ,  tenTi7=  "    «        "EPSILON   terms,    no   longer   used" 
<<    endl ; 

.' /  cout  --■  "term6="  <<   rho2  *  L3  '    X_qdsn   *  U*Q  *  del ta_planes_stern 
*  EPSILON  <<  endl ; 

//  cout  <-<  "term7="  <<   rho2  '  L2  ^  EPSILON  *  (  X_wdsn   *  U*W 

//  *  del ta_planes_stern 

/ -•  +  X_dsdsn  *  U2         *  del  ta_planes_stern 

//  *  delta_planes_stern) 

/ /  <<  endl ;                           » 

cout  <■■■    "terr-.c="  -,-  *    rho2  *  L2  *  C_dO  *   square  (speed_per_rpm) 

*   0.5  '  (   AU\'_t.ort_rpiTi  *  fabs  (AUV_port_rpin) 

+  AL^_stbd_rpm  *  fabs  (AUV_stbd_rpm) ) 
<■--  endl  ; 

cout  --••.  "terrri9="  <■:    -    rno2  *  L2  '  C_dC  *  U  *  fabs  (Uj 
-•<  endl; 

/  ,'  /  /  /  /  /■  .  •  /  /  /  ::  U  ;  1 1  1 1 1 1 1 1 1 1 1 1 1 1  N  n  1 1 1  i  1 1 1 1 11 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 11  n  1 1 1 1 1  n  1 1 1 1 1 1 1 
rhs  [SWAY  1  =  //  Sway   Motion  Equation  right  hand  side // 

in*  (-  (U*R)  +  (W*P)  -  x_G  *  (P  *  Q) 

+  y_G  *  (P2  +  R2) 
-  z_G  *  (Q   *  R) ) 
+  rho2  *  L4  *  (   Y_pq     *  P*Q     4  Y_qr    *  Q*R) 
+  rho2  *  L3  *  (   Y_up     •  U*P     +  Y_ur    *  U*R 

+  Y_vq     *  V*Q     +  Y_wp    *  W*P     +   Y_wr  *  W*R) 
+  rho2  *  L2  *  (   Y_uv     *  U'V     +  Y_vw    *  V*W 

+  U*fabs(U)  *  Y_uu_delta_rb  *  del ta_rudder_bow 
+  U*fabs(U)  *  Y_uu_delta_rs  *  del ta_rudder_sLern) 
-  swav  integral 
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+  (Weight  -  Buoyancy)  *  cosTHETA  *  sinPHI 

(2.0  /  (24.0  *  24.0))   //  each  thruster  2.0  lb  per  24V  signal  squared 

*  (   AU\'_bow_lateral    *  fabs  (AUV_bow_lateral ) 

+  AU\'_stern_lateral  *  fabs  (AUV_stern_lateral )  )  ; 

!  I  i  1 1 1 1 :  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

if  ■:TrJ-.-;-Z'.    //  £way  TRACE 

i 

cout    <-'    " '    sway    terinl  =  "    <<      m    *    ( -     (U    *    R)    +    (W    *    P) 

-  x_G    *     (P    '    Q) 

+   y_G    '     (P2    +   R2) 

-  z_G    *     (Q      *    R)  ) 

•-•       rrl.di; 

cout  ••■  "tenri:="  ■'••    -  rhc2  *  L4  '  (   Y_pq    *  P*Q    +  Y_qr    *  Q*R) 
•.••  ir.d:  ; 

cout  ■-:■.  "terrr:3="  <--    ^    rhc2  *  L3  "  i   Y_up     *  U'P    +   Y_ur   *  U'-R 

-  Y_vq    "   V'C    *    Y_wp.    *  W'F     *      'i_yir    *  W*R) 
<  •■'  e  n  2  J. ; 

ccur  •  ■:  "terTri4="  ••<-    +  rhc2  *  L2  *  (   Y_uv     *  U*V    +  Y_vw    *  V*W 

^  U'fabs(U)  •"  Y_uu_dei  ta_rb  *  del  ta_rudder_bow 

*  U*fabe(U)  *  Y_uu_del ta_rs  *  del ta_rudder_scern) 
<<  endl ; 

cout  «,-  "teiTTiB^"  ■-■    -  sway_integral 
<--  endl; 

ccuc  <•  "tenTib="  «    *    (Weight  -  Buoyancy)  *  cosTHETA  *  sinPHI 
•--  er.dl  ; 

cout    <-     "terni7="    ■•--         -       (2  .  0    /     ( 24  .  0    *    24  .  0 )  ) 

//    each   thruster    2.0    lb  per   24V   signal    squared 

*  (      AUV_bow_lateral         *    fabs    (AUV_bow_lateral ) 

+   Air/_stern_lateral    *    fabs    (AUV_stern_lateral ) ) 


1 1 1 1 1  i  i  I  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1  i  11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 11 1 1 

rhs  iKEAVEj  =  //  Heave  Motion  Equation  right  hand  side  // 

in  '  (   (U  *  Q)  -  (V  *  P)  -  x_G  *  (P  *  R)  -  y_G  *  (Q  •  R) 

+  z_G  *  ( P2  +  Q2 ) ) 
+  rho2  *  L4  *  (   Z_pp     *  P2      +  Z_pr  *  P*R     +  Z_rr  *  R2) 
+  rho2  *  L3  *  (   Z_uq     *  U*Q     +  Z_vp  *  V*P     +  Z_vr  *  V*R) 
+  rho2  *  L2  *  (   Z_uw    •  U*W    +  Z_w  •  V2 

+  (  U*fabs(U)  *  Z_uu_delta_b  *  delta_planes_bow   ) 
+  (  U*fabs(U)  *  Z_uu_delta_s  *  del ta_planes_stern) ) 
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-  heavfc_integral 

+  (Weight  -  Buoyancy i  *  cosTHETA  '  cosPHI 

^       .'   EPriLON  tierm?  have  been  removed  due  to  revised  equations  of  motion 

//  *    rho2  *  L3  *     Z_qn   *  U*Q  '  EPSILON 

//  *  rho2  *  L2  *  (   Z_wr,   ♦  U'-W 

//  +  Z_dsn  *  U*fabs(U)  *  del ta_planes_stern)  *  EPSILON 

+   (2.0  /  (24. C  *  24.0))   //  each  thruster  2.0  lb  per  24V  signal  squared 

*  (  AUV_bow_vertical    *  fabs  (AUV_bow_vertical)   + 
AUV_stern_vertical  *  fabs  (AUV_stern_vertical ) ) ; 

1 1 1 !  1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 H 1 1 1 1 1 

if  (TRACE)    //  Heave  TRACE 
{ 

cout  <^-  "*  heave  teiiTil^"  <<  m  *  (   (U  *  Q)  -  (V  *  P)  -  x_G  *  (P  *  R) 

-  y_G  *  (Q  '  R) 


<<•  endl  ; 


+  z_G  *  (P2  +  Q2) ) 


cout 

"  tei-ni5=  " 

<< 

<  <., 

end.  ; 

cout 

<i  •^' 

"  t  e  iTTi  6  =  " 
endl  ; 

<< 

cout 

<.^ 

"  terniT^  " 

<. 

<-  *, 

end!  ; 

cout  <■;  "tenn2-"  «    +  rho2  '  L4  *  (   Z_pp     *  P2      +  Z_pr     ■•  P*R 
-f  Z_rr  '  R2  :  <<  endl  ; 

cout  ■■-  "teriTi3="  <<    +  rno2  ■•  L3  *  (   Z_uq     *  U*Q     +  Z_vp  '  V*P 
-  Z_vr  '  V'R)  <<  endl ; 

cout  <<  "terTri4="  <<    *  rho2  '  L2  *  (   Z_uw     *  U*W     +  Z_vv   *  V2 

+  (  U'fatasfUi  ■•  Z_uu_delta_b  *  del  ta_planes_bow   ) 

+  (  U*fabs(U)  *  Z_uu_delta_s  *  del ta_planes_stern i j 
<-'  endl  ; 

-  heave_integral 

+  (Weight  -  Buoyancy)  *  cosTHETA  *  cosPHI 

"EPSILON  terms,  nc  longer  used" 

cout  <--  "term8="  «    +  rho2  *  L2  *  (  Z_wn   *  U*W  ) 
<<    endl; 

cout  «    "tenn9="  <<-    +   (2.0  /  (24.0  *  24.0)) 

//  each  thruster  2.0  lb  per  24V  signal  squared 

*  (  AUV_bow_vertical    *  fabs  {AUV_bow_vertical)   + 
AUV_stern_vertical  *  fabs  (AUV_scern_vertical )  ) 
«  endl; 
) 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1  // 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  // 11 1 1 1 1 1 1  n  1 1 1 

rhs  (ROLL  )  =  //  Roll   Motion  Equation  right  hand  side // 

-  (i_z  -  i-^")  *  Q*R  -  i_xy  *  P*R  +  i_yz  *  (Q2  -  R2)  +  i_xz  *  p*q 

-  rr.  *  (  y_G  -  (  -U*Q  +  V*P)  -  z_G   •  (  U*R  -  W*P)  ) 
+  rho2  *  L5   *  (   K_pq  *  P*0   +  K_qr  *  Q*R 
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+  K_pp  *  p  *  fabs(P) 

+  K_p   *  P  )       1 1   hovering  roll  drag 

-  rho:  "  L4   *  (  K_up  *  fabs(U;*P   +  K_ur  *  U'R   +  K_vq  *  V»Q 

+  K_wp  *  W*F   +  K_wr  *  W*R) 
+  rho2  *  L3   *  (   K_uv  *  U*V   +  K_vw  *  V*W 

-  U*fabs(U)  "  0 . 5  »  (   K_uu_planes  *  delta_planes_bow 

4  K_uu_planes  *  del ca_planes_stern) ) 
//  expected:  opposed  plane  directions  "  cause  negation  i<  cancellation 

+  (y_G  *  Weight  -  y_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 

-  (Z_G  *  Weight  -  z_B  '  Buoyancy)  ♦  cosTHETA  *  sinPHI; 

/ ,'  EPSILON  terms  have  been  removed  due  to  revised  equations  of  motion 
//  +  rho2  ■•  L4  *  K_pn  *  U*P  *  EPSILON 

T  rhoZ  ''    LI-  ■"  "J*fabs;U;   *  K_pTop;  //  oversimplified,  in  error 

.  / ,  /  ,• .'  :.:,;:  1 1 1 1:  n  1 1 1;,::  I  i  1 1 1 1 1  •  1 1 1 1 1:  i !  n  1 1  i  1 1 !  1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11  i  1 1 1 1 1 1 

^:  'Tr./*.  "E     .',  F.cll  TRACE 

:r,u:  •-■-  '"  roll  ternil  =  "  -<  -  ;:_z  -  I_y )  *  Q^R  -  I.x^--  '  P'R  +  I_yz  *  (Q2  -  R2 ) 

+  I_X2  '  P'Q 

•.••  end!; 

cout  <-•  '■cenT,2="  <-  -  m  *  (  y_G  '  (  -U«Q  +  V*P)  -  z_G   *  (  U'R  -  W*P)) 
^-  endl ; 

cout  <•-.-  "terT[i3="  «    +  rho2  *  L5   *  (   K_pq  *  P'Q   +  K_qr  *  Q*R 

*  K_p-p  '  F  *  fabs(P) 

■f  K_p   *  F  )   //  hovering  roll  drag 

<<  endi ; 

cout  <•'  "term4="  ---  *  rho2  *  L4   *  (  K_up   *  fabs(U)*P   +  K_ur  *  U*R 

*    K_vq  «  V*Q  *  K_wp  '  W*P   +  K_wr  •  W'R) 
■-•  endl; 

cout  <-•--  "ter:r.5="  --•■  -^  rhc2  *  L3   *  (   K_uv  *  U*V   +  K_vw  »  V*W 

-  U'fabsiU)  '  0 . 5  *  (   K_uu_planes  *    del ta_planes_bow 

+  K_uu_pianes  *  del ta_planes_stern) ) 
//  expected:  opposed  plane  directions  ""    cause  negation  &  cancellation 
<-  endl  ; 

cout  •:•  "term6="  •:-  -^  {y_G  *  Weight  -  y_B  *  Buoyancy)  *  cosTHETA  •  cosPHI 
<•-  <^ndi; 

cout  <^-  "term7="  <<    -     ( z_G  *  Weight  -  z_B  *  Buoyancy)  *  cosTHETA  *  sinPHI 
«  endl ; 

cout  «■  " term8 , term9= "  <<        "EPSILON  terms,  no  longer  used' 
<<  endl ; 

//  cout  <<  "termS^"  <<  +  rho2  '  L4  *  K_pn  *  U*P  *  EPSILON 
//      <<  endl ; 

//  cout  <<  •term9="  «  +  rho2  *  L3  *  U*fabs(U)   *  K_prop 

//     <<  endl ; 

) 


197 


:  i 1 :  1 1 1 1 1 1 11 /  u  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 / 1 1 11 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 i 1 1 1 1 1 1 1 1 / 1 1 1 1 1 1 1 1 n 
rhs  IPITCH]  =  //  Pitch  Motion  Equation  right  hand  side // 

-  ll_>:  -  I_Z/  '  P'R  ■»  I_xy  '  Q'R  -  \_jz    *  P*Q  -  I_xz  *  ( P2  -  R2) 
*      m  *  (  x_G  *  (  -V'-'Q   +  V*Pi  -  z_G   *  (  -  V'R  +  '*i''Q)) 

+  rho2  *  L5   *  (   M_pp  *  P2    +  M_pr  *  P*R   +  M_rr  *  R*fabs  (R) 

+  M_q   '  Q 

+  M_qq  *  Q  *  fabs(Q;)   //  hovering  pitch  drag 

+  rho2  '  L4   *  i  M_uq  *  U'Q   +  M_vp  *  V*P   +  M_vr  *  V'-R) 

^  +  rho2  *  L3   *  (  M_uw  *  U*W   +  M_w  *  V2 

+  U'^fabs(U)  "*  (   M_uu_delta_bow    *  del ta_planes_bow 

+  M_uu_delta_stern  *  del ta_planes_stern) ) 

+  pitch_integral   //  note  sign  corrections  to  Healey  pitch_integral 

-  (x_G  *  Weight    -   x_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 

-  (z_G  *  Weight   -   z_B  *  Buoyancy)  '  sinTHETA 

(2.0  /  (24.0  *  24.0))   //  each  thruster  2.0  lb  per  24V  signal  squared 

''/  multiplied  by  respective  inoment  arms 
//  x_bow_vertical  (+),  x_stern_vert  (-) 

'  (  (Al'V_bow_vertical   *  fabs  (AUV_bow_vertical )    *  x_bow_vertical ) 

+ (AUV_stern_vertical  *  fabs  (AUV_stern_vertical )  *  x_stern_vertical ) ) ; 

//  EPEILON  terms  have  been  removed  due  to  revised  equations  of  motion 

/,■  +  rho2  '  L4   *   M_qn  *  U'Q  *  EPSILON 

:'    +  rrio2  ^  L3   *  :;M_wn  *  U'W  +  M_dsn  *  U'fabs(U)  *  delta_planes_stern) 

//  •*  EPSILON; 

1 1 1 1 1 1 !  i  1 1 1 1 1  1 1 1 1!  1 1 1 1  It  1 1 1 1 1 n 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

if  fTPJ-.CE)    //  Pitch  TRACE 
{ 

cout  ■•-'  "•  pitch  tem-::="  <<    -    ( I_x  -  I_z)  '  P*R  *    I_xy  *  Q^R  -  I_y2  *  P*0 
-  I_xz  *  (P2  -  R2)  <<    endl ; 

cout  •--'  '■tenTi2="  <-<  +   m  '  (  x_G  *  (  -U*Q  +  V*P)  -  z_G   *  (  -  V*R  +  W*Q)  ) 
■-.-•'  endl  ; 

cout  «  "tenTi3="  <■--  +  rho2  *  L5   *  (   M_pp  *  P2   +  M_pr  *  P*R   +  M_rr 

*  R*fabs  (R) 
+  M_q   *  Q 

+  M_qq  *  Q  '  fabs(Q))   //  hovering  pitch  drag 
«  endl ; 

cout  «  "term4-"  «  +  rho2  *  L4   '  (  M_uq  *  U*Q  +  M_vp  •  V*P   +  M_vr  *  V*R) 
«  endl ; 

cout  «  "term5="  «  +  rho2  *  L3   *  (  M_uw  *  U*W  +  M_w  *  V2 


«  endl ; 


+  U*fabs(U)  *  (   M_uu_delta_bow   *  delta_planes_bow 

+  M_uu_delta_stern  •  delta_planes_stern) 


cout  «  "term6="  «  +  pitch_integral 
«  endl ; 

cout  ■-•:  "tenn7-"  <-.:  -  (x_G  *  Weight   -   x_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 
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«-  endl; 

cout  ---  "tenTi6="  •-<  -  ( z_G  *  Weiaht    -   2_B  *  Buoyancy)  *  sinTHETA 
•.-  end:  ; 

coLit:  <■'  "teiTn9="  «  +   (2.0  /  (24.0  *  24.0)) 

//  each  thruster  2.0  lb  per  24V  signal  squared 
//  multiplied  by  respective  moment  arms 
//  x_bow_vertical  (+).  x_stern_vert  (-) 

*  (  (AUV_bow_vertical    *  fabs  (AUV_bow_vertical )    *  x_bow_vertical ) 
+  (A'JV_stern_vertical  ■*  fabs  (AUV_stern_vertical )  *  x_stern_vertical )  ) 
<<   endl ; 

ccut  •••;  "  teririlO,  terml  1  = "  <<    "EPSILON  terms,  no  longer  used" 
<<  endl ; 

//  cout  ■^<    " terml 0="  «  +  rho2  ♦  L4   *   M_qn  *  U*Q  *  EPSILON 


" terml 0= " 

«  +  rho2  *  L4 

endl  ; 

"terml 1= " 

•  ■'  ^    rho2  '  L3 

*  EPSILON 

/.'  cOot  ■■<    "termi:  =  "  •  ■-  ^    rho2  '  L3   *  (M_wn  *  U'-W  +  M_dsn  *  U*fabs(U) 

*  del ta_planes_stern) 
*  EPSILON 
•;--'  ena.  ,- 
) 

t :  t ;!  i !  i  1 1  n  i  1 1 1 1 1 1 1 1  i  I  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

rhs  ;YAW   j  =  /  /  Yaw   Motion  Equation  right  hand  side // 

-  (I^-  -  I_x   *  P*G  +  I_xi'  *  (P2  -  Q2)  +  I_yz  '  P*R  -  I_xz  '  Q*R 

-  m  *  I  X_G  '  (  U*R  -  W*P)  -  y_G    *  (  -  V*R  +  W*Q) ) 

-  rho2  ■*  L5  '  (   N_pq  '  P*Q   +  N_qr  *  Q*R 

-  N_r   *  R 

+  N_rr  *■  R  *  fats  (R))   //  hovering  yaw  drag 

*  rhc2  ■•  L4   '  (  N_up  '  U*P   *  N_ur  *  U*R   +  N_vq  *  V*Q 

*  N_wp  '  W*F  *    N_wr  *  W*R) 

*  rho2  '  L5   '  (  N_uv  '  U*V   +  N_vw  •  V*W 

*  U*fabs(U)  *  N_uu_delta_rb  *  del ta_rudder_bow 

-  U*fabs(U)  *  N_uu_delta_rs  *  del ta_rudder_stern) 

-  yaw_integral 

■-  (x_G  '  Weigh:   -   x_B  *  Buoyancy)  *  cosTHETA  *  sinPHI 

*  (y_G  *  Weight   -   y_B  *  Buoyancy)  *  sinTHETA 

(2.0  /  (24.0  *  24.0))  //  each  thruster  2.0  lb  per  24V  signal  squared 

//  multiplied  by  respective  moment  arms 
*  (  (AU\<'_bow_lateral    *  fabs  (AUV_bow_lateral )    •  x_bow_lateral    ) 

+  (AU"v_stern_lateral  *  fabs  (AUV_stern_lateral )  *  x_stern_lateral  )) 

-  rho2  *  L2  *  C_dO 

*  (  square  (speed_per_rpm)  *   0.5  //  propeller  yaw 

*  (   AUV_port_rpm  *  f abs (AUV_port_rpm)  *  y_port_propeller 

+  AUV_stbd_rpm  *  f abs (AUV_stbd_rpm)  •  y_stbd_propeller ) 
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-  U  '  fabs(U) ) ; 

1 1 !  1 1 1 !!  1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

it     (TRACE)    //  Yaw  TRACE 

cout  •'■-  "'  yaw  tennl="  <<      -    (I_y  -  I_x)  *  P*Q  +  I_xy  *  (P2  -  Q2 ) 

+    I_j/2    *    F*R    -    I_xz    *    Q*R 
<■•-•   endl; 

cour    •■•     "term2="    -.••         -      m    •     (    x_G    *    (    U*R    -    W'P)    -   y_G        *    (    -    VR    +   W'Q)  ) 

••     end!; 

cout   •:<    "Cenn3="    «        +    rhc2    '    L5      *    (      N_pq   *    P*Q      +   N_qr    *   Q*R 

«»■  +    N_r      ■*    R 

!^  +   N_rr    ■•    R    *    fabs    (R)  )       //   hovering  yaw  drag 

-  -■•   end]  ; 

cour.    ■•■     "tenn4="    ---:        +    rho2    *   L4      *    (   N_up   *   U'P      +   N_ur   *   U*R      +   N_vq   •  V*Q 

+   N_wp   *    W*P     +   N_wr   *   W'R) 
<<  endl ; 

cout    «    "tenn5="    «    +    rho2    *    L3      *    (    N_uv   *   U*V      +   N_vw   *   V*W 

■>■   U*fabs(U)    '   N_uu_delta_rb   *    del ta_rudder_bow 

-  U*fabs(U)    »•   N_ud_delta_rs    *   del!:a_rudder_stern) 
•-■-   endl; 

-  yaw_integral 

rout:  --•••  "c^i7Ti7="  <-•   +  (x_G  ■*  Weight   -   x_B  *  Buoyancy)  ■•  cosTHETA  *  sinPHI 
-."•"  endl; 

cout  -:--  "teriTie^"  ---'   +  (y_G  *  Weight   -   y_B  *  Buoyancy)  *  sinTHETA 
■-■"  er:d^  ; 

cout  «    "teniri9="  <-<  -   (2.0  /  (24.0  *  24.0)) 

//  each  thruster  2.0  lb  per  24V  signal  squared 
//  multiplied  by  respective  moment  arms 
*  .;  (ALry_bow_lateral    *  faos  (AUV_bow_lateral )    *  x_bow_lateral    ) 

-^  (A'JV_stern_lateral  *  fabs  (AUV_stern_lateral )  *  x_stern_lateral  )) 
-----  endl; 

cout  <■-    "tenTilO="  «  -  rho2  *  L2  *  C_dO 

'  (  square  ( speed_per_rpm)  ■*   0.5  //  propeller  yaw 

*  (   AUV_port_rpm  *  f abs (AUV_port_rpm)  *  y_port_propeller 

-I-  AUV_stbd_rpm  *  f  abs  (AUV_stbd_rpm)  *  y_stbd_propeller ) 

-  U  *  fabs(U) ) 
«  endl ; 

) 

1 1 1 1 1 1 1  i  i  1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

II    debug: 

if  (TRACE) 


"  tei"nic  = 

" 

end:; 

"  teiTTi7  = 

" 

endl ; 

"teniie^ 

■• 

er:d^  ; 

cout  <<    " 

SURGE 

= 

"  << 

SURGE 

«  endl ; 

cout  ■--<    " 

SWAY 

= 

"  << 

SWAY 

<<  endl; 
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:cdC 
rout 
:our 
:oar 


HEAVE 
POLL 
PITCH 
YAW 


-cut  ■ •  "rhs  [SURGE]  - 

cout  ■<  "rhs  [SWAY  ]  = 

couc  ^<  "rhs  [HEAVE]  = 

cout  <*  "rhs  [ROLL  ]  = 

cout  ■'<-  "rhs  [PITCH]  = 

cout  -<  "rhs  [YAW   J  = 


«  HEAVE  «  endl 

<<  ROLL  <<  endl 

«  PITCH  «  endl 

<<  YAW  <<  endl 

«  rhs  [SURGE]  «  endl 

<<  rhs  [SWAY  ]  «  endl 

«   rhs  [HEAVE]  «   endl 

«  rhs  [ROLL  ]  «  endl 

<<  rhs  [FITCH]  «  endl 

<<  rhs  [YAW   ]  <<  endl 


//  cout  <<  "mass_inverse : "  <<  endl;    print_matrix6x6  (mass_inverse ) 


cout  <<  "velocities: 


/ 


cout 


■RHE 


<<  U  <<  " ,  "  <<  V  «  " ,  "  «  W  <<  " ,  " 

<<  P  <<  " ,  "  <<  Q  <<  " ,  "  <<  R  <<  ">"  <<  endl ; 

print_matrix6  (rhs); 


'J.U 
0  .  u 


//  rhs  [SURGE] 

rr.o  !  SWAY  i 

'  rhj  [HEAVE ]  =  0.0 

■  .  rns  [P.CLL  ]  =  0.0 

rns  ; YAW   '  =  0 .  u 

i  i  i  :  1 1 1 :  I !  1 1 1  !  1 1 1 1 1 1 1 1 1 1 1 !  1 1  /  1 1  i  1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 11 1 1 1 1 11 1 1 1  n  /  1 1 11 1 1 1 1 1 

/.■    ;alculatt   .ntw   accelerations   matrix   using   mass_inverse   i    rhs,    print // 

n,ul t If  ly bxc_6    !niass_inverse,    rhs,    new_acceleration)  ; 

1 1     :  Tr.ACE ; 

cout  <<    "Accelerations:   " ;    print_matrix6  (new_acceleration) ; 


.irr.it  accelerations 


claiTipfS,  new_acceleration  (SURGE),  -5.0,  5.0,  "new_acceleration  [SURGE]") 

ciampii  new_acceleration  [SWAY  ],  -5.0,  5.0,  "new_acceleration  [SWAY  ]") 

clan.pii  ntw_acceJeration  [HEAVE],  -5.0,  5.0,  'new_acceleration  [HEAVE]") 

ciariipti  new_acceleration  [ROLL  ),  -5.0,  5.0,  "new_acceleration  [ROLL  ]") 

ciampii,  new_acceleration  [PITCH],  -5.0,  5.0,  "new_acceleration  [PITCH]") 

clamp  (S(  new_acceleration  [YAW   1,  -5.0,  5.0,  "new_acceleration  [YAW   ]") 


•// 


//  find  velocities  by  integrating  averaged  accelerations 
//       (Heun  integration) 


new_Vf loci ty 
new_veloci ty 
new_veloci  ty 
new_vel DC i  ty 
new_veloci  ty 
new_veloci ty 


SURGE]  = 

0 

5 

*  {u_dot 

SWAY  ]  = 

0 

5 

•  (v_dot 

HEAVE]  = 

0 

5 

*  (w_dot 

ROLL  ]  = 

0 

5 

*  {p_dot 

PITCH]  = 

0 

5 

*  (q_dot 

YAW   ]  = 

0 

5 

*  (r_dot 

new_acceleration 
new_acceleration 
new_acceleration 
ne w_a  cceleration 
new_acceleration 
new  acceleration 


[SURGE] 

*  dt 

+ 

U; 

[SWAY  ] 

*  dt 

+ 

V 

[HEAVE] 

*  dt 

+ 

W 

[ROLL  ] 

*  dt 

+ 

P 

[PITCH] 

*  dt 

+ 

Q 

[YAW   ] 

•  dt 

+ 

R 

// 


//  find  velocities  by  integrating  instantaneous  accelerations 

//       (this  method  is  less  accurate  and  is  not  used,  although  at  small 

//       timesteps  the  difference  is  negligible) 

//      (Euler  integration) 


//  new_velocity  [SURGE] 
//  new_velocity  [SWAY  ] 


=  (new_acceleration  [SURGE])  *  dt  +  U; 
=  (new_acceleration  [SWAY  ])  *  dt  +  V; 
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/'  new_velociCy  (HEAVE] 

//  new_velocity  [ROLL  1 

/.•  new_ve:ocicy  [PITCH] 

/,'  nt^w_vilocity  [YAW   ) 


(new_acceleration  [HEAVE])  *  dc  +  W 

(new_acceleraLion  [ROLL  ])  *  dt  +  P 

inew_acceleraCion  [PITCH])  *  dt  +  Q 

;new_acceleracion  [YAW   ] )  *  dt  +  R 


,,  iiOCfc  that  suigt  velocity  may  be  negative  under  to  model  constraints 
//  but  this  is  a  problem  so  it  is  clamped  to  be  non-negative 

clamp  (4.  new_velocity  [SURGE],  -3.0,  3.0,  "new_velocity  [SURGE]  velocity"); 

//  3.0  is  set  arbitrarily  high 

,' •  upJjte  UUVBody  Jtate  accelerations  to  newly-calculated  values   // 

u_dot  =  new_acceleration  [SURGE] 

v_dot  =  new_acceleration  [SWAY  ] 

w_dot  =  new_acceieration  [HEAVE] 

p_dot  =  new_acceleration  [ROLL  ] 

.:i_do;-  =  new_acceleration  [PITCH] 

r  dor  =  new  acceleration  [YAW   ] 


.' /  calculate  world  coordinate  system  linear  &  angular  velocities 


•// 


//   see  Cooke  Figure  10  for  corrections  to  Healey  equations  for  x/y/z_dot: 
//   also  Healey  course  notes  eqn  (26)  and  Frank-McGhee  corrected  paper  (A. 8) 

x_dot  =  AL'\'_oceancurrent_u 

-^  U  *   cos  I  PS  I)  '  cos  (THETAi 

+  V  '  (cos  (PSI)  *  sin  (THETA)  *  sin  (PHI)  -  sm  (PSI)  *  cos(PHI)) 

+  W  »  (cos  I  PSI)  *  sm  (THETA)  *  cos  (PHI)  +  sin  (PSI)  '  sin  (PHI)); 
y_dc:  =  Airv_oceancurrent_v 

^  U  '   sin  (PSI)  *  cos  (THETA; 

+  V  »  isin  (PSI)  '  sin  (THETA)  '  sin  (PHI)  +  cos  (PSI)  *  cos (PHI)) 

-f  W  '  (sin  (PSI;  *  sin  (THETA)  '  cos  (PHI)  -  cos  (PSI)  *  sin(PHI)); 
z_dot  =  AUV_oceancurrent_w 

-  u  '  sin  (THETA) 

+  V  »  cos  (THETA)  *  sin  (PHI) 

4-  W  *  cos  (THETA)  *  cos  (PHI); 

phi_dot    =  P  +  Q  *  sin  (PHI)  *  tan  (THETA) 

+  R  *  cos  (PHI)  *  tan  (THETA); 

theta_dot  =  Q  *  cos  (PHI) 

-  R  *  sin  (PHI) ; 


if 


) 


(cos  (THETA)  ==  0.0) 

cout  «  "UUVBody :  :  integrate_equations_of_niotion  ():  "  <<  endl ; 
cout  «  "  cos  (THETA)  ==  0.0  so  psi_dot  set  equal  to  zero."  <<  endl; 
psi_dot  =  0.0; 


else  psi_dot 


[Q  *  sin  (PHI)  +  R  *  cos  (PHI))  /  cos  (THETA); 
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Veccor3D   linear_rates    =      Vector3D    (x_dot,    y_dot,    z_doc); 

if     (TRACE; 

t 

:. cu  z    ■  ■     cfTidl  ; 

cout  •'■-•  "•:x_dot,  y_doc,  z_doc->         =  "  <<  linear_rates  <<  endl  ; 

ccut  <-,  "  magnitude  ~    "  <<  linear_rates  .magnitude  { 


;■--  end 


1  . 


} 


Vertcr3D  euler_rates  =   VectorBD  (phi_doc,  theta_dot,  psi_dot); 

if  (TRACE) 

( 

cout  <<  "<phi_dot,  theta_dot,  psi_dot>  =  "  «   euler_rates  <<  endl ; 

cout  «  "  magnitude  =  "  «  euler_rates .magni tude  () 

<•-  endl  ; 
) 

//  calculate  world  coordinate  system  homogenous  transform  matrix    // 

Hjiiatiix  Hiiicremerital  =  Hmatrix  i  )  ;  //  default  initialization 
Hincremiental  .  set_posture  (  P  *  dt,  Q  '  dt,  R  *  dt  )  ; 
Kincremental . rotate       (  PHI,     THETA,   PSI     ) ; 
doub!i.fe  w_x  =  Hincremental  .phi_value    () 
douDle  w_j-  =  Hincremental  .  tneta_value  i) 
double  w_z  =  Hincremental . psi_value    () 

Ve.-r  c  1  31  w:):ld_rareo  =   VectorBI  (w_x,  w_y ,  w_z )  ; 

If  (TRACE 

I 

ccut  <■•  "<w_x,  w_\',  w_z  >  =  "  <<   world_rates  <<  endl  ; 

ccut  <■'    "  magnitude  =  "  <<   world_rates  .magni  tude  () 

< •  endl ; 

ri-'i.atiix  Hrevisedl  =  Hmiatrix  u;    //  default  initialization 

Hrevisedl . incremental_rotation     (  phi_dot,  theta_dot,  psi_dot,  dt  ); 

Hrevisedl . incremental_translation  (  U,  V,  W,  dt  ) ; 

Hmatrix  Hproductl  =  Hprevious  *  Hrevisedl; 

Hmatrix  Krevised2  =  Hmiatrix  (    x_dot  *  dt,     y_dot  *  dt,    z_dot  *  dt, 

phi_dot  *  dt,  theta_dot  *  dt ,  psi_dot  *  dt ) ; 

Hprevious  =  Hproductl; 

//  translate  and  rotate  and  update  time  in  RigidBody  state  // 

//  note  world  coordinate  system  is  used  by  RigidBody: 

set_angular_veioci ties  {phi_dot,  theta_dot,  psi_dot); 

set_l inear_veloci ties  i   x_dot,     y_dot,    z_dot); 

set_time_of_posture  ( current_uuv_time) ; 

update_Hmatrix  (dt); 

If  (TRACE) 
( 

cout  <<  "incremental  hmatrix  =  "; 

Hincremental .print_hmatrix  (); 

cout  <<  "revisedl  hmatrix  =  "; 

Hrevisedl .print_hmatrix  (); 

cout  <<  "revised2  hmatrix  =  "; 

Hrevised2 .print_hmatrix  (); 

cout  <<■    "productl  hmatrix  =  "; 
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Hproductl .print_hmatrix    (); 

cout    <.•-    "original    hinatrix  = 
hinatrix.print_hina!:rix    ( )  ; 


cout    v-^    "substituting  productl    hinatrix"    «  endl; 
hmatrix   =   Hproductl; 


// 


.'J^ve  body-coordinate-system  velocities  for  the  next  loop: 


u  = 

new_ 

.veloci  ty 

[SURGi 

^  1 

V  r 

new. 

.velocity 

[SWAY 

1 

w  = 

new. 

.velocity 

[HEAVE] 

F  = 

new. 

.velocity 

[  ROLL 

1 

Q  = 

new. 

.velocity 

[PITCH] 

F;  = 

!"iew. 

.v.^ioci  ty 

(YAW 

1 

/  / 

c  ou  t 

.'. 

"world  U 

-  "  ■  •  - 

U 

<  <    " 

,  x_dot     = 

« 

x_dot 

« 

endl 

cour 

.  <■ 

"world  V 

=  "  <<' 

V 

<<    " 

,  y_dot 

<< 

y_dot 

« 

endl 

/  / 

cout 

<'  <~ 

"world  W 

=  "  << 

w 

<<  " 

,  z_dot 

« 

z_dot 

« 

endl 

// 

cout 

*-'<' 

"world  P 

=  "  «,-< 

F 

<<  " 

,  phi_dot 

« 

phi_dot 

<< 

endl 

// 

cout 

<< 

"world  Q 

=  "  << 

Q 

<<  " 

,  theta_dot  ^ 

« 

theta_dot 

« 

endl 

// 

cout 

« 

"world  R 

=  "  << 

R 

<<  " 

,  psi_dot 

« 

psi_dot 

« 

endl 

//  

//  update  ail  hydrodynarr;ics-model -provided  state  variables  in  AUV_globals .  h 
//        prior  to  retransmittal  to  AUV  via  AUVsoc>:et 


AUV_time 

=  current 

_uuv_tiine  ; 

Airv'_x 

=  x_value 

0 

// 

A'.Jv^v' 

=  y_value 

0 

/  / 

ALn/_z 

=  z_value 

0 

// 

AL"'»'_phi 

=  phi_val 

ue    (  ) 

// 

Au"/_theta 

=  theta_v 

alue  () 

// 

AL;v_i.31 

=  psi_val 

ue    0 

/  / 

AUV  u 

new. 

.velocity 

[SURGE] 

// 

KJ^_-J 

new. 

.velocity 

[SWAY  ] 

// 

AW  w 

new. 

.veloci  ty 

[HEAVE] 

// 

ALrv_p 

new. 

.veloci  ty 

[ROLL  1 

// 

AjV  q    = 

ncw. 

.veloci  ty 

[PITCH] 

/  /' 

Air^_r 

new. 

.velocity 

[YAW   ] 

// 

AUV_u_dot 

=  u_dot; 

// 

Airv_v_dot 

=  v_dot; 

// 

AUV_w_dot 

=  w_dot; 

// 

ALrv'_p_dot 

=  p_dot; 

// 

AUV_q_dot 

=  q_dot; 

// 

AUV_r_dot 

=  r_dot; 

// 

AUV_x_dot 

=  x_do  t ; 

// 

AUV_>'_dot 

=  y_do  t ; 

// 

AUV_z_dot 

=  z_dot; 

// 

AUV_phi_dot 

=  phi_dot 

; 

// 

AUV_theta_ 

.dot 

=  theta_dot; 

// 

AUV_psi_dot 
) 

=  psi_dot 

'■ 

// 

#undef  UDOT 

#undef  VDOT 

«undef  WDOT 

#undef  PDOT 

#undef  QDOT 

x    position  in  world  coordinates 

y    position  in  world  coordinates 

z    position  in  world  coordinates 

roll   posture  in  world  coordinates 

pitch  posture  in  world  coordinates 

yaw   posture  in  world  coordinates 

surge  linear  velocity  along  x-axis 

sway   linear  velocity  along  y-axis 

heave   linear  velocity  along  x-axis 

roll   angular  velocity  about  x-axis 

pitch  angular  velocity  about  y-axis 

yaw   angular  velocity  about  z-axis 

linear  acceleration  along  x-axis 

linear  acceleration  along  y-axis 

linear  acceleration  along  x-axis 

angular  acceleration  about  x-axis 

angular  acceleration  about  y-axis 

angular  acceleration  about  z-axis 

Euler  velocity  along  North-axis 

Euler  velocity  along  East-axis 

Euler  velocity  along  Depth-axis 

Euler  rotation  rate  about  North-axis 

Euler  rotation  rate  about  East-axis 

Euler  rotation  rate  about  Depth-axis 
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#ur.dfef  ROOT 

#uridef  SURGE 

#undef  SWAY 

«-^ndef  HEAVE 

*'j!i..-j^:  PITCH 

«„rjjt-:  YAW 

// // 

iendif   //  U1.TVE0DY  C 
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AUVsocket.C  Cominunications  with  a  Networked  AUV 


'program:  AUVsocket.C 

uescripcion:       socket  from  auv  execution  level  Co  Irix  auv  virtual  world 
receives  partial  telemetry,  returns  full  telemetry  with 
dynamics  parameters  added 

ALiVsocket.C  is  a  function  library  used  by  UUVBody 

A'JVsocket  also  provides  voice  server  interface 

Revised:  26  October  94 

Compilation:       unix--  make  AlTVsocket 

Original  bases:     os9server.c,  dynamics. c 

References:     (1)  (Gespac-provided)  EVIRA  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2!  Internetworking  with  TCP/IP  Volume  I:   Principles, 
Protocols  and  Architectures,  Douglas  E.  Comer, 
Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

i3}  Internetworking  with  TCP/IP  Volume  II:   Design, 

Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ ,  1991 

(4)  IRIX  Network  Programming  Guide,  Silicon  Graphics  Inc. 

(5)  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 
Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 
Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 

;6;  Real-Timie  Programmiing  Tutorial,  Bill  Mannel ,  SGI  Expo, 

Silicon  Graphics  Inc.,  23  May  93 
(7)  "Say..."   Axel  Belinfante's  speech  server  at  University 

of  Twente,  Netherlands,  which  also  uses  Nick  Ing-Simmons' 

phoneme  synthesizer  'rsynth': 

http://utisl79.cs .utwente.nl :8001/say/? 

Status:  Tested  satisfactorily 

Futuit  wcrK:       Consider  implementation  as  an  inetd  ' superserver'  daemon  or 

just  closing  socket  when  done,  reopening  &  waiting 

Consider  bounds  diagnostic  checking  on  values  transferred 
over  socket 

Fix  type  miismatch  on  signal  ()  calls  -  very  gnarly  problem! 

Comments  and  suggestions  are  welcome! 
V 

tifndef  AUVSOCKET_C 

#define  AUVSOCKET_C    /*  prevent  errors  if  multiple  #includes  present  */ 


/' 


:*«**«******************************************************♦************ 


#define  _BSD_SIGNALS 
^include  <ctype.h> 
#include  <signal.h> 
^include  <stdio.h> 
♦(include  <sys/types  .h> 
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^include  "-'sys / socket .  h> 

# Include  <neciner /in .h> 

«ir;r]'jde  <netdb.h  • 

♦tinclude  -  serine,  h-. 

*in-rlud>T  -'time.ir.- 

sinrl.de  -Scdntj.h-- 

#in;lude  -unisrd. h> 

»»^:.cludt  "AUyglobais.H" 

*inciudT  "Quaternion . C" 


//  global  state  vector 

//  degrees /'radians  conversions 


//  extern  "C"  entries  provide  function  compatibility  with  C++  compiler 


extern 


i 


void  bzero    (void  *b,  int  length) ; 

void  bcopy    (const  void  *src,  void  *dst,  int  length); 

int  strncmp  (const  char  *sl,  const  char  *s2,  size_t  n); 

char  'strcpy   (char  *sl,  const  char  *s2); 

chai  'strncat  (char  *sl,  const  char  *s2,  size_t  n); 

,  /■      refereiice   p.    434    Kelley   i    Pohl    'A   Book   on   C", 

//     /usr /inciude/sys/signal .h  and  /usr/include/signal .h 


//  mt  I 'signal  (  int ,  int  (*;(int. 


)  )  (mt. 


■  *■*«****■ 


*****«■ 


*****! 


,  '  One  L^trean.  socket  is  used  with  adequate  throughput 

."    .altr.ough  two  could  work,  no  performance  improvement  is  expected) 

/'  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
/■"     frcn  other  processes  req-uesting  ports  on  your  system.: 


n  jtf.:.^  1  :£EF.:L)GE_TCP_PDR'] 
»j^f.:-..r  N'FSNET  MCA£T  PORT 


2056  /*  disbridge  1.3  prograiTi,  server  &  client 

3111  /*  Mike  Macedonia's  multicast  DIS  2.0.3 

/*  NP£  Autonomous  Underwater  Vehicle  (AUV) 
Underwater  Virtual  World  (UVW) 


*/ 
*/ 

*/ 


#def ine 

AUVSIMl 

TCP 

PORT  0 

3210 

/* 

«def  me 

au^'sim: 

TCP 

PORT  1 

3211 

/* 

«define 

AUVSIMl 

_TCP_ 

.P0RT_2 

3212 

/• 

#define 

AUVSIMl 

TCP 

P0RT_3 

3213 

/* 

«def  me 

AUVSIMl. 

TCP 

PORT  4 

3214 

/■' 

#define 

AUVSIMl. 

TCP 

PORT  5 

3215 

/* 

#define 

AU^'SIMl. 

_TCP_ 

.P0RT_6 

3216 

/* 

#def ine 

AUVSIMl. 

-TCP. 

PORT  7 

3217 

/• 

•define 

AUVSIMl. 

_TCP. 

PORT  8 

3218 

/' 

1  de  I  1  ;  Itr 

AU-\;SIM1. 

-TCP. 

PORT  9 

3219 

/' 

#defmtr  SOCKET_QUEUE_SIZE 


os9sender  <==>  os9server  test  programs 
auv  execution  level  <==>  virtual  world 
auv  execution  <==>  tactical  (networked 

port  for  future  use 

port  for  future  use 

port  for  future  use 

port  for  future  use 

port  for  future  use 

port  for  future  use 

port  for  future  use 


/'  max  allowed  by  TCP/IP 


«def me   AUVSIM_EXECUTION_LEVEL_HOST_NAME    "auvsiml .or.nps.navy .mil " 


/' 


**«♦******■ 


:*******! 


r**«*»»*****««*, 


■♦******* 


/*  function  prototypes  ****•*********■'*«****««*»«*»**»**«**»*««**•»»«»•***»»* 
int     open_execution_level_socket       ( )  ; 

int     shutdown_socket  (int  signal_thrown) ; 

int     read_f rom_execution_level_socket  ( ) ;   //  returns  -1  on  failure 


*  / 
'/ 

*/ 

V 

)*/ 

*/ 

*/ 
*/ 
*/ 
*/ 
*/ 

•/ 


**/ 
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void  write_tc_Gxecution_level_socket  ( ) ; 

double  normalize  (double  degs) ; 

double  normalize2  (double  degs) ; 

void  AU\'_global_ini  tialization  (); 

/*  global  variable  definitions  *♦****»»»****«♦»*♦***♦■•****♦*»***♦****•*«*»»**»*/ 

static  int  socket_descriptor, 

socket_accepted, 
socket_streani; 

static  int  socket_length  =  255;   /*  max  allowed  packet  size  */ 

static  int  bytes_received,  bytes_read,  bytes_written, 

bytes_left,    bytes_sent; 

static  cnar  command_sent  [300] , 

cormiand_received  [300], 

coininand_copy  [300], 

reinote_host_naine  [60]; 

static  int  shutdown_signai_received  =  FALSE; 

static  FILE  *  checksoundf ile; 

static  struct  sockaddr_in  server_address; 

static  struct  hostent    *  server_entity ; 

static  cnar  keyword  [81); 

static  cnar  *  ptr_index; 

static  cnar  '  remote_buf f er ; 

static  int  stace_variables_receivGd; 

static  int  recurn_state_vector; 

static  char  www_execution_message_string  [300]; 

static  cnar  answer; 

static    int  socket_alreadi'_opened   =    FALSE; 

staric    int  audio_enabled  =   TRUE; 

int  open_execution_level_socket  (}       /*  Initialize  communications  blocks  */ 
I 

if  (socket_already_opened  ==  TRUE) 
{ 

if  (TRACE) 
i 

cout  <<  "open_execution_level_socket  ():   socket_already_opened, " 
<<  "  returning"  <<  endl ; 
) 

return  (-1 ) ; 
} 
else  if  (TRACE)  cout  «  "open_execution_level_socket  ()  starting"  <<  endl; 

/*««««««♦♦*»♦«*♦*♦«♦««»*♦♦♦******«*«♦*****«♦***«*****«*****«*♦****»*»«**«»««««*/ 
/*  Initialize  both  client  £c  server  ****«******»*********«***•*«»***»****«*.»*«»/ 

/♦  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
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/*     signal  handlers.   Otherwise  you  are  unable  to  ^C  kill  this  program. 


«i f  defined (sgi ; 

•■  /  sign.^il  (SIGHUP,  shutdown_socket ) 

/,   iigji^a  (iloINT,  3h'LJtdOW!',_SOCkfet  ) 

y '■  sign-;;!  (£:IGKILL,  shurdown_socket  ) 

//  signal  (EIGFIPE,  shutdown_socket ) 

//  signal  (SIGTERM,  shutdown_socket ) 

*endi  f 


/*  hangup  */ 

/*  interrupt  character  */ 

/*  kill  signal  from  Unix  */ 

/*  broken  pipe  from  other  host  */ 

/*  software  termination  */ 


/  %    Tiiicialize   ■-server    »********»*«»**«**«**«************************************/ 

/*  setup  to  listen  for  client  to  attempt  connection  */ 

/ -  Server  opens  server  port  **«»««»»*****♦•*»»*»*»**«»»*•**•*'**«•»••••»*/ 

/"  Open  TCP  (Internet  stream)  in  socket  */ 

if  (  (socket_descriptor  =  socket  {AF_INET,  SOCK_STREAM,  0))  <  0  ) 

I 

cout  ^^-  "open_execution_level_socket  ()  can't  'open'  stream  socket" 
<■■  end!  ; 

return  (- 1 ) ; 
i 
e:=e  .f   TRACE) 

cout  '^<     "open_execut ion_levei_socket  ()  socket  'open'  successful" 
<<   endl ; 
) 

/*  Bind  local  address  so  client  can  talk  to  server  */ 

«: 1  defined . sgi , 

bzero  '  'vo:d  *;  &server_address ,  sizeof  ( server_addre3s ) ) ; 
*end: f 

r?'r'iver_addrei=s  .  sin_f  ami  ly      =  AF_INET;    /*  Internet  protocol  family  */ 

/*  nia<t  sure  port  is  m  network  byte  order  */ 

3T:-ver_addres?.sin_addr  .3_addr  =  htonl  ( INADDR_ANY)  ; 
server_address. 3in_port        =  htons  (AUVSIM1_TCP_P0RT_1 ) ; 

i:  !i:,.''id  .  socket_descriptor , 

(sti'L^ct  sockaddr  *;  iserver_address , 

sizeof  (server_address ) )  <  0) 
( 

cout  --•-:  "open_execution_level_socket  ()  socket  'bind'  unsuccessful" 

<•'  endl  ; 
return  (- 1 ) ; 
) 

else  if  (TRACEl 
( 

cout  <-<-  "open_execution_level_socket  ()  socket  'bind'  successful" 
«  endl ; 
} 

/*  prepare  socket  queue  for  connection  requests  using  listen  */ 

listen  (socket_descriptor,  SOCKET_QUEUE_SIZE) ; 

if  (TRACE) 

{ 

cout  «  'open_execution_level_socket  ()  socket  'listen'  complete  ..." 
<<   endl ; 
} 

/*  Server  'accept'  waits  for  client  connections  ***»«•**•*****»»***•*•**»/ 

if  (TRACE) 

{ 
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couc  «  "open_execution_level_socket  ()  socket  waiting  to  'accept' 
<■  eiidl  ; 
1 

bytes_received  -  sizecf  ( socket_descriptor) ; 
while  ( (socket_accepted   =  accept  (  socket_descriptor , 

Siserver_address, 
ibytes_received) )  <  1) 
if  (TRUE)   /*  blocking  code  */ 


cout  -i-.:  "open_execution_level_socket  ()  socket  " 

<<  "'accept'  unsuccessful,  sleeping  1  second  . 
sleep  ( 1 ] ; 


<<  endl ; 


cout  <v;  "open_execu tion_level_socket  (/  connection  is  open  between 
«  "networks."  <<  endl; 


i  /'  end  initialization 
socket_streani  =  socket_accepted; 
socket_al ready _opened  -   TRUE; 

if  'TRACE; 


/*  server  */ 


1 


cout  <<.  "AUVsocket  SERVER:  socket_descriptor  = 
<<   endl, 

socket_accepted 


cout    '-< 

■<  endl 

cout    <■<  " 

<<  endl 


socket  stream 


«   socket_descriptor 
<<  socket_accepted 
<<  socket  stream 


return  (TRUE) ; 


/""*■'• 


int    rt=ad_f roiTi_execution_level_socket 
( 

/'  temporary  hold  variables  */ 


//  using  global  variables  state  vector 


AUV_z_temp, 
AUV_psi_temp, 
AUV_w_t  emp , 
AUV_r_temp, 
AUV_z_dot_temp, 
AUV_p s i _d o t _t emp , 


double   AUV_time_temp , 

AU"v_x_temp,  AUV_y_temp, 

AUV_phi_temp,  AU\'_theta_temp, 

AUV_u_temp,  AUV_v_temp, 

AUV_p_cemp,  AUV_q_temp, 

AU\'_x_dGt_temp,  AUV_y_dot_temp, 

AUV_phi_dot_temp,  AUV_theta_dot_temp, 

AUV_del ta_rudder_temp ,  AUV_deita_planes_temp 

AUV_port_rpm_temp,  AUV_stbd_rpmi_temp, 

AUV_bow_vertical_temp,  AUV_stern_vertical_temp, 

AUV_bow_lateral_temp,  AUV_stern_lateral_temp, 

AUV_ST1 000_range_temp ,  AUV_ST72  5_range_temp , 

AUV_ST100  0_bearing_temp,  AUV_ST72  5_bearing_temp, 

AUV_ST1000_strength_temp,AUV_ST72  5_strength_temp; 
int      start_index,  offset,  index; 

/*»*»«*****■»*«****************«*«******«*****«*****************»**«**«******/ 

/*  Receiver  block  */ 

/***************************************************************************/ 

for  (index  =  0;  index  <  300;  index++)      /*  uppercase   */ 
command_received  [index]  =  (char)  0; 

/*  listen  to  remote  host,  relay  to  local  network /program  */ 
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bytes_lefc        =  socket_length; 

by!:ef_received    =  0  ; 

pCL"_.r:d^-x         =  command_received  ; 

w|-_:^  'ibyrt3_ieft  -0)  &S,  ( by  ces_received  >=  0))   /'  read  loop  •♦»*'«»'»'*»/ 
i 

bytes_read  =  read  ( socKec_stream,  pLr_index,  bytes_left); 

if       (by"es_read  <-   Oi  bytes_received  =  bytes_read; 

else    it    (byces_read  ^   0; 

i 

fcytes_left  -=    bytes_read; 

bytes_received  *=  bytes_read; 

ptr_index      +=  byces_read; 
J 

if  (TRACE) 
( 

cout  <<-  '  read_f  rom_execucion_level_socket  ()  '; 

couc  <■'    "receiver  block  loop  bytes_read  -    "  <<  bytes_read<<  endl  ; 
) 

/ '  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop 
if  ( (bytes_read  ==  0)  &&  (bytes_received  ==  0))  break; 
) 

::    l:yres_rece:ved  <  0;       /*  failure     */ 

if  (TRACE; 

cout  <--  "  read_f  roin_execution_level_socket  ()  "  ; 
cout  <<  "receiver  block  read  failed,  bytes_received  -    ' 
<•-   bytes_received  <<  endl  ; 

retur:"._3tat<^_vector  -    FALSE; 
;''^ri;iT:   I'-^rurn  state  vettcr;; 


else    if    (byt es_received   --   0)      /*    no   transfer    */ 

if     :  TRACE/ 

cout  •'■--  "  read_f  roiT,_execution_level_sockeL  ()  "; 

ccut  ■■  "receiver  block  by tes_received  =   0  bytes"  <<  endl; 

return_state_vector  =  FALSE; 
return  {return_state_vector ) ; 

/*   (bytes_received  >  0)  =>  socket  read  OK,  print  result  if  valid  &  traced  */ 

if  (TRUE)   //  print  telemetry  record  &  key 

cout  <<  cominand_received  «  endl; 

cout  «  "keyword, t,x,y, z, phi , theta,psi , u, v,w, p, q, r,  " 

«  "x_dot,y_dot, z_dGt,phi_dot , theta_dot ,psi_dot, " 

«  "delta_rudder , del ta_p lanes,  l_rpm,  r_rpin,  " 

<<  "bow_vertical ,bow_lateral , " 

<<  "stern_vertical , stern_lateral ,  " 

«   "ST1000_bearing/range/strength, " 

«  "ST725_bearing/range/strength"  «  endl; 
) 

state_variables_received  =  sscanf  ( comma nd_received, 

"%s  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf 
%lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  \n", 
keyword,  &AUV_time_temp, 

&AUV_x_temp,  &.AUV_y_temp ,  &AUV_z_temp, 

&ALTV_phi_temp,  &.A(JV_theta_temp,         &AUV_psi_temp, 
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£iAU\'_u_temp, 
&A'JV_p_temp, 
ScAUV_x_dot_t  emp , 
5tA'JV_phi_doc_temp, 
uh'v\'_del  ta_rudder_temp, 
4cA'.A/'_port_rpm_tenip , 
S(;AUV_bow_ver c  i  ca  l_temp  , 
&.AUV_bow_l  a  t  er  a  l_t  emp , 
6tAUV_ST100  0_bearing_tenip, 

&Ai-'V_ST7  2  5_bea  r  i  ng_t  emp , 


ScAUV_v_temp, 
5cA'JV_q_temp, 
&AUV_y_dot_temp , 
&AUV_theta_dot_temp, 
6tAUV_de  1 1  a_pl  anes_temp , 
&  AU\'_s  t  bd_rpm_t  emp , 
5iAUV_stern_vertical_temp, 
iAUV_stern_laceral_temp, 
&AUV_ST1 O00_range_temp , 
S.AUV_ST1 0  00_s  t  rengt  h_temp , 
S,AU\'_ST72  5_range_temp, 
ScAUV_ST7  2  5_strength_temp)  ; 


&AUV_w_temp, 
ScAUV_r_c  emp , 
£cAUV_z_dot_temp, 
ScAUV_p  s  i  _d  o  t  _t  emp , 


for  (index  =  0;  index  <  strlen  (keyword);  index++) 
keyword  [index]  =  toupper  (keyword  [index]); 


/*   uppercase 


V 


if     (TRACE;    princf    '  "  [AU\'sockeC    keyword=%s  ]  \n  " ,    keyword); 

if  (L-t  are_var:  ables_received  ==  34)   //  transfer  was  OK  so  keep  new  values 


TRACi 


print f 


[AUVsocket    scate_variables_received   ==    34j\n"); 


re:urri_£ 

AUV_time 

A'J-.'_x 

AlTv-_y 

A'A'_2 

AU\'_p)hi 

AUV_thec 

Air/_psi 

AiJV_u 

A'SJ_v 

AlTi'_W 

AU\'_p 

AlT\'_q 

A'JV_r 

AU'y_x_do 

AU"v'^_do 

AUV_z_do 

AU\''_phi_i 

AUv'_thfe 

AlP.'_ps:_i 

Air;_del  t 

AUV_delt 

A'JV_porC 

AU'v_stbd. 

AUV_bow 

AU\.'_ster 

ALrv_Dow 

AW_ster 

AU^;_ST1 0 

AUV_£T10 

AUV_ST10 

AU\'_ST7  2 

AUV_ST7  2 

AUV    ST7  2 


cace   vector 


TRUE; 


t 
dot 

dot 
dot 

a_rudder 
a_planes 
_rprTi 
Lrprn 
vertical 
n_vertical 
lateral 
n_lateral 
0  0_be  a  r 1 n  g 
00_range 
00_strength 
5_bearing 
5_range 
5_strength 


radians 
radians 
radians 


radians 
radians 
radians 


radians 
radians 
radians 
radians 
radians 


AUV_time_temp; 

AUV_x_temp; 

AU\'_y_temp; 

AUV_z_temp; 
(AUV_phi_temp) ; 
(AUV_theta_temp) ; 
(AUV_psi_temp) ; 

AUV_u_temp; 

AUV_v_temp; 

AUV_w_temp ; 
(AUV_p_temp) ; 
(AUV_q_temp) ; 
(AUV_r_temp) ; 

AUV_x_dot_temp ; 

AUV_y_dot_temp , 

AUV_z_dot_temp; 
(AUV_phi_dot_temp) ; 
(AUV_theta_dot_temp) ; 
(AUV_psi_dot_temp) ; 
(AUV_delta_rudder_temp) ; 
(AUV_delta_planes_temp) ; 

AUV_po  r  t_r pm_t  emp ; 

AUV_3 1  bd_rpiTi_t  emp ; 

AUV_bow_vertical_temp; 

AU\'_stern_vertical_temp; 

AUV_bow_lateral_temp; 

AUV_stern_lateral_temp; 

AUV_£T10  00_bearing_temp; 

AUV_ST10  00_range_temp; 

AUV_ST10  00_st rengt h_temp; 

AUV_ST72  5_bearing_temp; 

AUV_ST72  5_range_temp; 

AUV_ST72  5_st  rength_temp ; 


else   /*  a  message  was  received  instead  of  telemetry  *, 

{ 

if  (TRACE)  printf  ("[AUVsocket  non- telemetry  received] \n" ) ; 

return_state_vector  =  FALSE; 

if  (strcmp  (keyword,  "AUV_STATE")  ==  0) 
( 

printf  ( 
"Garbled  telemetry  record  received  ! ! !  variables_received=%d\n%s\n" 
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state_variables_received,    coinmand_received)  ; 
f flush    I scdout ) ; 

recurn_state_vector     =  FALSE; 
TRACE  =  TRUE; 

cm. get  , answer);  //  paase 
cour  • -  endl ; 

else  if  (strcmp  (keyword,  "MAIL")  ==  0)      /*  Send  e-mail  request  */ 
I 

print  f    ("%s°n",    cominand_received)  ; 

system  (        command_received) ; 
I 
else  if  !strncmp  (command_received,  "KILL",  4)  ==  0) 

/■*  KILL  prior  to  reloop*/ 
{ 

if  (TRACE)  printf  ( " [AUVsocket  KILL]\n"); 

shutdowr,_socket  (0); 

returr,_state_vector  =  -3; 
) 

else  if  (Strcmp  (keyword,  "TIME")  ==  0) 
{ 

if    (TR.ACE)    printf    ( "  [AlP/socket   TIME]\n"); 

state_variables_received  =  sscanf  (command_received,  "%s%lf", 

keyword, 
&AU\'_'^iin&_temp)  ; 

if    (state_variables_received  ==2)      //    transfer  OK,    keep  new  values 
( 

Airv_time  =    AUV_time_temp; 

return_state_vector  -    FALSE; 

prj.ntf    ;"%s\n",    coirjriand_received;  ; 


printf    ;"a    bad   time   commiand  was   received  i<    ignored:      %s", 

comrr.and_received) 
return_state_vector   =    FALSE; 
> 

strcmp  (keyword,  "POSITION")  ==0)  II 
'Strcmp  (keyword,  "LOCATION")  ==  0)  ) 

if  .'TP^CE.i  printf  {  "  [AlA'socket  POSITION]  \n"  )  ; 

state_variables_received  =  sscanf  (command_received,  "%s%lf %lf %lf  ' , 

keyword,     &AUV_x_temp, 
i.AUV_y_temp ,  icAU\'_z_tempj  ; 

if    (state_variables_received  ==4)      //    transfer  OK,    keep  new  values 
{ 

AUV_x  =   AUV_x_temp; 

AUV_j/  =   AUV_y_temp; 

AlP/_z  =   AUV_z_temp; 

returri_state_vector   =   -4; 

printf    ("%s\n",    command_received) ; 
) 

else 
{ 

printf  ("a  bad  position  was  received  i  ignored:   %s", 

command_recei ved ) 

return_state_vector  =  FALSE; 
) 
} 
else  if  {(strcmp  (keyword,  "ROTATION")     ==0)  II 
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(scrcrrip    (keyword,     "ORIENTATION")    ==    0)) 
{ 

state_variables_received  =    sscanf    (coinmand_received,    "%s%lf %lf %lf " , 

keyword,  tAUV_phi_temp, 

&AUV_theta_temp,    SJVUV_psi_temp) ; 

if    (state_variables_received   ==4)       //    transfer   OK,    keep   new  values 
{ 

AUV_phi  =    radians    (AUV_phi_tenip)  ; 

AUV_theca  =    radians    {AUV_theta_temp) ; 

AU\'_psi  =    radians    (AW_psi_tenip)  ; 

retL:rn_stace_vector   =    -  4  ; 
printf    ("%s'.,n",    corranand_received)  ; 
)^ 

else 
1 

printf     I  "a   bad   orientanion   was    received   &    ignored:      %s", 

command_received) ; 
returrj_state_vector    =    FALSE; 
} 
) 

eli-e    if     (srrncmp    ( comjriand_recei  ved,     "AUDIBLE",    7)    ==0)    /*    enable   sound   */ 
\ 

printf  ("[AUVsocket  AUDIBLE] \n" ) ; 

return_state_vector  =  FALSE; 

audic_enabled       =  TRUE; 

i'trtjrn    ( return_state_vecLor  i  ;       /■*    duplicate   conmiand   copy   not    spoken   */ 

eli^e    if    (strncmp    (cominand_recei ved,     "SILENT",    6)    ==    0)    /'   disable   sound   */ 

printf     ;  "  (AWsocket    SILENT]  \n" )  ; 

return_state_vector    =    FALSE; 
audio_enabled  =    FALSE; 

} 

if    ((strncinp  (keyword,  "AU\''_STATE" ,  10)  !=  0)  /*  non- telemetry  string  */ 

&£<  (audio_enabled  ==   TRUE)  j 
J 

/ '  generate  audio  of  non-telemetry  line  passed  by  execution  level   */ 

/  ■"  L)on  Brutzman  Naval  Postgraduate  School   brut zmanSnps  .navy  .mil   */ 
,**»**«»«»■■»**»****««*»»»******«»**«***«******««»***«*«****»***««*«**/ 

start_index  =  0; 
offset  =  0; 
strcpy  (command_copy ,  ""); 

/*  leading  blanks  in  query  are  stripped  */ 

for  (index  =  0;  index  <=  strlen  (command_received) ;  index  ++) 

i 

if   (command_received  [index]  ==  '  ' )  start_index  =  index  +  1; 
else  break; 

) 

/*  clean  up  any  extra  stuff  in  the  query  string  */ 

//  if   (command_received  [strlen  (command_received) ]  ==  '\n') 

//         command_received  [strlen  (command_received)   ]  =  ')'; 
//         command_received  (strlen  (command_received) -1]  =  ' { ' ; 

int  linelength  =  strlen  (command_received) ; 
command_received  [linelength]  =  (char)  0; 

//  printf  ("  command_received  [linelength]  =  {%d}\n", 
//  command_received  [linelength]); 

for  (index  =  start_index;  index  <=  linelength;  index  ++) 
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I 

if       (  (coiniTiand_received  [index]    ==  '  '  )  ii 
(cominand_received  [index  +  lj  ==  '  ')) 
offset  =  offset  -  1;  //  ignore  multiple  blanks 
else  if  (  (cominand_recejved  [index]    --    '    '  )  && 
(isdigit  icoinmand_received  [index+1]))) 
( 

cominand_copy  [index  -  start_index  +  offset]  =  ','; 
/■/  don't  put  minus  before  number,  use  comma  instead 
) 
else  if   (command_received  [index]  ==  '  ' ) 

command_copy  [index  -  start_index  +  offset]  -    '-'; 
else  if   (commiand_received  [index]  ~-    '\n') 
( 

command_copy  [index  -  start_index  +  offset]  =(char)  0  ; 
break; 
J 
else  command_copy  [index  -  start_index  +  offset]  = 

tolower    (coiTimand_received    [index]); 
) 

printf  ("  Say...%s\n",   command_receivedj ; 
if  (TRACE;  printf  ( "  [Say  .  .  .  %s] \n  " ,  command_copy ) ; 

/■'  build  the  remote  g-uery  which  will  also  save  results  as  -object   */ 

strcpy  ^keyword,  "speech/"!; 
strcat  (keyword,  comrTiand_copy )  ; 
strcat  '.keyword,  ".au"); 

checksoundf ile  =  fopen  (keyword,  "r"); 

if  ;  checKsoundf  ile  ==  !^IULL)  / »  file  not  previously  retrieved-do  so  */ 

If  (TRUE 

printf  .:"i9cL-  not  found,  making  remote  query  to  \n",  keyword); 
printf  t"  tne  Say.,  audio  server  in  Netherlands ] \n" ) ; 
) 
sprint f  1  www _execution_message_st ring, 

"wwv    -c  spfech/%s.au  http : / /www_tios . cs . utwente .nl/say /?%s" , 

command_copy ,  command_copy i ; 

if  .TrJiCE;  printf   ("[%Si\n",  www_execution_message_string) ; 
systen.   (www_execution_message_string ) ; 
I 

else 
( 

if  (TRACE) 

printf  i"'%s  was  found  locally,  no  remote  query  required] \n" , 
keyword; ; 
fclose  (checksoundf lie) ; 
1 

/*  build  string  to  play  the  audio  file,  then  do  so.  */ 

/*  don't  put  sfplay  in  background  or  audio  port  may  be  unavailable  •/ 
/*       when  the  next  audio  message  is  sent  */ 

sprintf  {www_execution_message_string,  "sfplay  speech/%s . au" , 

command_copy ) ; 
if  (TRACE)  printf  ("[%s]\n",  www_execution_message_string) ; 
system   (www_execution_roessage_string) ; 

/*  audio  send  complete  ******•**************************•*****•**•**/ 
) 
/*  end  processing  a  message  * / 
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return  ( recurn_stace_vecCor)  ; 

*  end  read_f rom_executicn_level_socket  ( ) ; 


/ 


void  write_to_execution_level_socket  ()   //  using  global  variables  state  vector 

sprint  f  (conimand_sent , 
■'  uvw_state  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
15. 3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
%5.3f  %B.3f  %5.3f  %5.3f  %5.3f  %5.3f  %d  %5.3f  %5.3f  \n", 

AUV_time,  AUV_x,  AUV_y ,  AUV_2 , 

ncriTiaIize2  (degrees  (AUV_phi)), 

normalizel  (degrees  (AUV_thetaM, 

norir.alize   (degrees  (AUV_psi)j, 

AUV_u,  AUv'_v,  AUV_w, 

normalize2  (degrees  (AUV_p)), 

normalize2  (degrees  {AU^v'_q)  )  , 

normalizel  (degrees  (AUV_r) ) , 

AU\"_x_dot,   AUV^_dot,   AUV_2_dot , 

norinalize2    (degrees    (AUV_phi_dot !  )  , 

ncriinalize2    (degrees    (AU'\/_theta_dot )  )  , 

ncrmalize2  (degrees  (AU\'_psi_doi: )  )  , 

normalize2  (degrees  i AU\'_del ta_rudder )  )  , 

norn"'iaii2e2  (degrees  ( Airv'_de  it  a_p  lanes  ;;  , 

A:".'_i:  c  r  t_rpn: ,        AW_3 1  bd_rprr, , 

AU"v'_bow_vertical ,     AUV_stern_vertical , 

AU''v'_bow_lateral ,     A'J\'_stern^lateral , 

ALr;_£T:000_bearinq,   AUV_ST100C_range,     AUV_ST1000_strength, 

AU\'_£T7  2  5_bearing,        Airv_ST72  5_range,  AUV_ST725_strength 

if    (TRACE;    cout    <<   comrriand_senc    <<   endl  ; 
/*    Sender   block  */ 

/**»****************«*****»************«*********«***********************«**; 

by tes_received  =  strlen  (cominand_sent )  ; 

if  ( Dy tes_received  <  0)   /*  copy  failure  */ 

I 

cout  <-:  "write_to_execution_level_socket  ()  "  ; 

cout  <•;  "copy  froni  command_rec6ived  unsuccessful"  <<  endl; 

snutdown_socket  (0;; 

return; 
} 

else  if  {bytes_received  >  socket_length) 
i 

cout  <<  "write_to_execution_level_socket  ()  " 

<<  "send_telemetry_to_server  error:  "  <<  endl; 
cout  <<    "bytes_received  too  big  for  packet  socket_length" 
<<  "  [bytes_received="  «  bytes_received 

<<  "]  >  (socket_length= "  «  socket_length  <<  •];  " 
<<  "string  truncated"  «  endl; 
} 

bytes_left        =  socket_length; 

by tes_wri tten  =    0; 

ptr_index  =   coiTiniand_sent ; 

while    ( (bytes_left    >   0)    &&    (by tes_written   >=    0))       /*   write   loop   **« 
i 

bytes_sent    =   write    (socket_stream,    ptr_index,    bytes_left); 
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******** 


if      (bytes_senC  <  0)  bytes_writter,  =  bytes_sent; 
eise  if  {bytes_sent  >  0) 

byte£_lefc  -:=    bytes_seni: 

byLe3_wri tten   +=  bytes_3ent 

ptr_index      +=  bytes_sent 

) 

if  (TRACE  S(&  (byces_writcen  !=  sockec_length)  ) 

{ 

couc  -•  "wri  te_to_execL;tion_level_sockeC  ()  loop   bytes  sent 
<•'  bytes_sent  <■■   endl  ; 
J 
} 

if    (by tes_written  --   0) 
i 

cout  -.'•-  "wri  te_to_execution_level_socket  ()  send  failed,  " 
<«'  byces_written  <'-  "  bytes_written"  «  endl; 

else  if  (TRACE) 


cout  ■:•  "wriCe_to_execution_level_socket  ()  total  bytes  sent  = 
«  bvtes  written  «   endl ; 


}. 


return; 

'*  end  wri te_tc_execution_level_socket  (); 


:******* 


int   3hu:dGwn_sGcket  (int  signal_thrown; 


/*  Shutdown  block 


int  cicse_result; 

if  .TRACE: 

:-cu:  ••<■  "shutdcwn_socket  '"  <<   siqnal_tr.rown 
-■'  " .:  shutdown  in  progress  ..."  <<   endl; 

3nutdcwn_sign3l_received  -   TRUE; 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down, 
/'     Since  EIGPIPE  Signal  trigger  may  cause  shutdown  on  other  side 

close_result  -   close  ( socket_stream) ; 

if  iclose_result  ==  -1) 

cout  ••  "shutdown_socket  k)    close  {socket_stream)  failed"  <<  endl; 

if  (TRUE)  cout  <<    "shutdown_socket  ()  complete"  «  endl; 

return  (close_result ) ; 

)  /*  end  shutdown_socket  ()  */ 


*/ 


double  normalize    (double  degs ) 
{ 

double  result  =  degs; 


■♦♦**♦**»♦«««***♦*****♦♦***♦*♦*»«»,,♦«♦**/ 


/»  degrees  input*/ 


while  (result  <    0.0)  result  +=  360.0; 
while  (result  >=  360.0)  result  -=  360.0; 
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rtL'jrr.  result; 


:*«*«! 
:**«*: 


:****«****«*********»***«»**/ 
:«»***»««»«***«**«***»«*»***/ 


double  normalizeZ  (double  degs) 
1 

double  result  =  degs; 

while  (result  <=  -180.0)  result  * 
while  (result  v    18  0.0)  result  - 


/*  degrees  input'/ 


=  360.0, 
=  360.0, 


return  result; 

vcid  .VoV_globai_ini tialization  () 

/'/  Variable  initialization  performed  separately  to  avoid  compiler  warnings  -  - 

//  mission  time 

//  positive  is  bow  rudder  

//  positive  is  bow  planes  


AU'/_time 

- 

0 

0 

AW\I_del  ta_rudder 

= 

0 

0 

AUV_del ta_planes 

= 

0 

0 

AU\,'_lJGrt_rpm 

= 

0 

0 

Al"v'_scbG_rpm 

- 

0 

0 

A  W_bow_ver  t  i  ca  1 

= 

c 

0 

AU'v'_3 1  e  r  n_v  e  r  t  i  c  a  1 

= 

0 

0 

AlJV_bow_laterai 

= 

0 

0 

AU\'_stern_lateral 

^ 

0 

0 

AUV_depth_cel 1 

_ 

0 

0 

AlPy_heading 

= 

0 

0 

AUV_roIi_angle 

= 

G 

0 

AU\''_p  i  t  c  h_a  ng  1  e 

= 

0 

0 

AU"v'_roil_rate 

= 

0 

0 

AUV_pitch_rate 

= 

0 

0 

AUV_yaw_rate 

= 

0 

0 

AUV_hour 

= 

0 

AUV_m,inute 

- 

0 

ALrv"_second 

- 

0 

//  propellor  revolutions  per  minute 

//  propellor  revolutions  per  minute 

//  thruster  volts  24Y  =  3820  rpm  no  load 

//  thruster  volts  24V  -    3820  rpiri  no  load 
//.thruster  volts  24V  =  3820  rpm  no  load 

//  thruster  volts  24V  =  3820  rpm  no  load 

//  pressure  sensor,  units  are  

//  gyrocompass  in  degrees 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  matches  intertial  sensor  onboard  AUV 

//  internal  AUV  OS-9  system  time,  unused 


/  Hydrodynamics-model-provided  state  variables 


AUv'_x 

AU^/^' 

AUV_z 

AUV_phi 

AUV_theta 

AUV_psi 

AUV_x_dot 

AUV_y_dot 

AU'\/_z_dot 

AUV_phi_dot 

AUV_theta_dot 

AUV_psi_dot 

AUV_u 
AUV_v 
AUV_w 
AUV_p 
AUV_q 
AUV_r 


0.0;  //  X    position  in  world  coordinates 

0.0;  //  y    position  in  world  coordinates 

0.0;  //  z    position  in  world  coordinates 

0.0;  //  roll   posture  in  world  coordinates 

0.0;  //  pitch  posture  in  world  coordinates 

0.0;  //  yaw   posture  in  world  coordinates 

0.0;  //      Euler  velocity  along  North-axis 

0.0;  //      Euler  velocity  along  East-axis 

0.0;  //      Euler  velocity  along  Depth-axis 

0.0;  //  Euler  rotation  rate  about  North-axis 

0.0;  //  Euler  rotation  rate  about   East-axis 

0.0;  //  Euler  rotation  rate  about  Depth-axis 

0.0;  //  surge  linear  velocity  along  x-axis 

0.0;  //  sway   linear  velocity  along  y-axis 

0.0;  //  heave  linear  velocity  along  x-axis 

0.0;  //  roll   angular  velocity  about  x-axis 

0.0;  //  pitch  angular  velocity  about  y-axis 

0.0;  //  yaw   angular  velocity  about  z-axis 
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AU\'_u_dot 
ATJ\'_v_dot 
A'J\'_w_doc 
AW_p_dor 
AUV_q_dot 
AUV_r_doc 


AUV_oceancurrenc_u  =  0.0 
AUV_oceancurrent_v  =  0.0 

AUV  oceancurrent  w  =  C.C 


0 

0 

// 

0 

0 

// 

0 

0 

// 

0 

0 

// 

0 

0 

// 

n 

0 

// 

linear  acceleration  along  x-axis 

linear  acceleration  along  y-axis 

linear  acceleration  along  x-axis 

angular  acceleration  about  x-axis 

angular  acceleration  about  y-axis 

angular  acceleration  about  z-axis 


//  Ocean  current  rate  along  North-axis 
//  Ocean  current  rate  along  East-axis 
//   Ocean  current  rate  along  Depth-axis 


//  Prior  time  loop  saved  variables 


AU\''_t  i  me_p  r  i  o  r 

AUV_x_prior 
AlA,'_y_prior 
A'J\'_z_ijrinr 

AUV_p'hi_prior 

AUV_theta_prio: 

AlT\'_psi_prior 


=  0.0;  //  mission  time 


0.0 
0.0 
0.0 

0.0 
0.0 
0.0 


//  x    position  in  world  coordinates 

//  y    position  in  world  coordinates 

//  z    position  in  world  coordinates 

//  roll   posture  in  world  coordinates 

//  pitch  posture  in  world  coordinates 

//  yaw   posture  in  world  coordinates 


I !   Sonar-  &  terrain-modei-provided  state  variables 


AU\_£T100C_bearing  =  0.0;  //  ST_1000  conical  pencil  beam  bearing 
AU\'_£T:000_range   =  0.0;  //  ST_1000  conical  pencil  beam  range 

AU\'_£T100  0_3trength=  0.0;  //  ST_10G0  conical  pencil  beam  strength 


AlTv'_ST7  2  5_Dearing  =0.0 
Al^_S'T'';'E_range  =  C:  .  0 
A'J\'  ST7  2  5  strenath  =  0.0 


// 

ST  725 

1 

X 

24 

/■/ 

ST  725 

X 

X 

2  4 

// 

ST  725 

1 

X 

24 

sector  beam  bearing 
sector  beam  range 
sector  beam  strength 


«e:.d; 


.V".'"')CKET 
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G.  DISNetworkedRigidBody.C  DIS  Network  Connections  for  a  Rigid  Body 

I  !  1 1  i  1 1  U  1 1 1 1 1  /  I  i  1 11 1 1 1  I II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
/* 
Prcgram:         DlSNetworkedRigidBody .C 

Description:  DIS  network  interface  for  RigidBody  model 

Author:  Don  Brutzman 

Revised:  28  October  94 

Sy  j:  t  em  :  I  r  i  x  5  .  2 

Compiler:  ANSI  C*+ 

Compilation:      irix>  make  dynamdcs 

irix>  CC  DISNetworkedRigidBody.C  -Im  -c  -g  +w 

Original  bases:   os9server.c,  dynamics. c,  DIS  library  test_it.c 

References:   (1)  IEEE  Protocols  for  Distributed  Interactive  Simulation  (DIS) 
Applications  version  2.0,  Institute  for  Simulation  and 
Training,  Universit  of  Central  Florida,  Orlando  Florida, 
2  e  May  19  9  3. 

i2)  Macedonia,  Michael,  Zeswitz,  Steven,  and  Locke,  John, 
Distributed  Interactive  Simulation  (DIS)  multicast 
version  2.0.3,  Naval  Post-graduate  School,  February  94. 

i3)  Zeswitz,  Steven,  "NPSNET:  Integration  of  Distributed 

Interactive  Simulation  (DIS)  Protocol  for  Communication 
Architecture  and  Information  Interchange."  master's  thesis. 
Naval  Postgraduate  School,  Monterey  California,  28  May  1993. 

Units:  Virtual  world:   ft    ft/sec    radians    radians/sec 

DIS  PDUs:       m     m/sec   degrees   degrees/sec 

Future  work : 

Advisors:         Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 
'/ 

1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

*ifndef  DISNETWORKEDRIGIDBODY_C 

#define  DISNETWORKEDRIGIDBODY.C  //  prevent  errors  if  multiple  #includes  present 

#define   METERS_PER_FT   0.3048 
#define   FT_PEP,_METERS   3.2808 

♦  include  "RigidBody.' .  C" 
♦include  "AUVsocket .C" 
♦include  <string.h> 
♦include  <bstring.h> 
♦include  <time.h> 

//  DIS  includes.   See  Makefile  for  other  DIS  ♦include  files;  they  must  match. 

♦include  " . . /DIS .mcast /h/disdef s . h" 
♦include  " . . /DIS .mcast/h/appearance .h" 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 
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//  DIE  library  function  prototypes  dc  not  include  parameter  prototypes,  thus 
/ /      needed  ones  are  included  here.   Function  definitions  are  located  in 
/■  /      /'n/elsie/work3 /macedoni /net/mcast /network/src 

i  i ;  i !! i  i ; I  i ! I  n  i  I  i  1 1 1 1 1 1  n  1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 !/ 1  n  1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

class  DISNetworkedRigidBody  -.  public  RiqidBody 
private : 


''   meniDer   dat 
double 
mt 

Entity  IE: 
Entity Type 
EntityStatePDU 
clock_t 

double 

mt 


ields 


time_of_last_PDU, 

DIS_iJort_open; 

UU\'_r;IS_id; 

Uirv'_DIS_type; 

UU\_DIS_pdu; 

current_clock, 
mi  tialclock , 
previous_clock , 
delta_clock; 
delta_time; 

PDU_skip_interval ; 


//  clock  tick  counter  for  current  loop 


//  Multicast  Defaults  from 

/ /  /n ' el  31 e  'work 3 /macedoni /net /mcast /network/uti Is /planes /planes . cc 


cnar 
~nar 
char 
char 
a  "  n  c  r 


MULTICA£T_PORT  icj 

MULTI CA£T_GROUF  (20; 

port  (6] 

group  [20] 


//  pointer  to  MllLTICAST_PORT 
//  MULTI CAST_GROUP 
//  time-to-live 


cue  lie: 

n.emb-^r  consrrucror  and  destructor  functions 
DISNetworkedRigidBody  ( ) 

-DISNetworkedRigidBody 

//  operitors 

frienc:  ostrcami  operator  << 


)  (  /•  null  bod\'  */  ) 


//  inspection  methods 

void  print_networkedrigidbody 

double  time_of_last_PDU_value 

int  DIS_port_opGn_value 

int  PDU_skip_interval_value 

//  modifying  methods 

void  set_PDU_skip_interval 

int  DIS_net_open 


(ostreami  out, 
DISNetworkedRigidBody^  nrb)  ; 


( )  const; 

( )  const; 

()  const; 

()  const; 

(const  int  new_value) ; 
0  ; 
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iiiL  Cil£_nfet:_wri  te  {); 

void  DI£_net_close  ( )  ; 

void  DISNecworkedRigidBody_init:ialize  (); 

void  set_C ime_of_lasc_PDU  {const  double  new_tinie_of_last_PDU) ; 

void  set:_ttl  (int    new_ttl); 

void  set_group  (char  *  new_group) ; 

void  set_port  (char  *  new_port); 
); 

!!  I  I  !  i !! it  I n  i I  I i 1 1  1 1 ! i 1 1  1 1 1 1 ! 1 1 1  I  I  1 1 / 1 1  1 1 1  11  1 1  1 1 1 1 1 1  1 1 1 1 1 1 1 1 1 1 1  I H 1 1 / I  1 1  1 1 1 1 1 1  I 

/  // 

//  constructor  methods 

// // 

DISNetworkedRigidBody : :  DISNetworkedRiqidBody  ()  //  default  constructor 

?rv_.?.k.:p_interval  =  1;  //  1  tenth  (s)  of  a  second  between  PDUs 

RigidBody_ini tiali ze  (  )  ;  //  inherited  method 

tinit_of_last_PDL'  =  0.0; 
DIS_port_open  =  FALSE; 

/'/  initialize  clock  variables 
^n: : .  aiclock  -  clock  U; 
py^^'i  ous_c  1  ock    =  clock  ;  i  ; 

//  Multicast  Defaults  from 

/ /  /n/elsie/work3/macedoni /net /mcast /net work /uti Is /planes/planes .cc 

bzerc  (MiJLTTCAST_PORT,  6); 

bzero  (MULTICAST_GROUP,  20); 

strcpy  (1»:ULTICAST_P0RT,  "3111");   ■      //  3111  npsnet  'standard'  address 

strcpy  (MULTICA£T_GROUP,  "224.2.121.93"); 

bzerc  (port ,    6 ) ; 

bzero  (qroup,  2  0 ; ; 

strcpy  (port,  MULTICAST_PORT) ; 

strcpy  (group,  MULTICAST_GROUP) ; 

:tl    =  16;             //  multicast  ttl=16  stays  inside  NPS 
} 
)  I // 

//  operators 

// // 

ostream^  operator  <<    (ostreamSr  out,  DISNetworkedRigidBody&  nrb) 
{ 

nrb.print_rigidbody  (); 

out   «    " time_of_last_PDU  -    '      «   nrb. time_of_last_PDU  «  endl ; 

return  (out ) ; 

) 

// // 

//  inspection  methods 

// // 

void  DISNetworkedRigidBody.:  print_networkedrigidbody  () 

const 

{ 
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print_rigidbody  ■: )  ; 

cour    --<-    "t  ime_of_last_PDU        =    "      <<    ciine_of_last_PDU  <<   endl  ; 

I 

// // 

double  I'lSNetworkedRigidEody :  :  Cii:ie_of_last_PDU_value  () 

const 

I 

return  Cin"ie_cf_last_PDU; 

// // 

int  DISNerworkedRigidBody : :  DIS_port_open_value  {) 
con?t 

1  er  u  rn  [)]  ?'._j:>crr_oper,  ; 

I 

,  I // 

inc  EiISNecworkedRiqidBody  :  :  PDU_skip_interval_value  () 

( 

re  1  u  rn  PEiU_s  k i  p_i  n  t e rva  1  ; 
) 
/, // 

■' /  modifyina  methods 

// : // 

void  DISNetwcrkedRigidBody : :  set_PDU_skip_incerval  (const  int  new_value) 
{ 

if    ( PEU_3kip_interval  >=  1'  PDU_skip_interval  =  new_value; 

ei  se 

i 

p:';_skip_interval  =  10; 

cout  •  ■'  "PE!U_skip_interval  must  be  >=  1,  and  has  been  set  to  1."  <<  endl  ; 

return; 

)  ; // 

mr    r  lEN'etworkedRig.dBcdy :  :    DIS_net_open    ij       //    Ref :    macedonia   include   files 

m:    exercise_rd       =  -1; 

int    coordinate_systein  =  0;   //  o  =  flat  world,  1  =  round  world 

char  ■*  utm_f  ile  =  "  " ; 

ir'  r^sulr  =  FALSE: 

if  !DI£_:Jort_open  ==  FALSE; 

result  =  net_open  (port,  group,  ttl, 

exercise_id,  coordinate_system,  utni_file)  ; 

//  result  ~   net_open  (port,  group,  ttl);  //  old  version 

if   (result  ==  FALSE) 
{ 

DIS_port_open  =  FALSE; 

cout  «  "DIS_net_open_select  ()  failed"  «  endl; 
1 

else 
( 

DIS_port_open  =  TRUE; 

//  atexit  (net_close) ;  //  ensure  port  is  reclosed  on  exit. 
) 
) 
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else 
I 

cout  <<    "DIS_port_open  ==  TRUE  so  DIS_net_open  ()  not  re-attempted." 
<<  endl ; 

i-c--^-.u:t  =  TRUE; 

letuiT!  result; 


II- 


■II 


int  DISNetworkedRigidBody : :  DIS_net_wri te  ( 


current_clock 
del ta_clock 
delta  time 


=  clock  ( ) ; 

=  current_clock  -  previous_clock; 

=  delta_clock  /  CLOCKS_PER_SEC; 


if  (TRACE)  cout  «  "DISNetworkedRigidBody :  .■DIS_net_write  :AUV_time  = 
«   A'JW    time; 


//  do  not  write  faster  than  one  DIS  PDU  per  PDU_skip_interval  tenth-seconds 

//■     of  AU\"_time 

if  '  I  int  I     (AU"v_time  '  10.0)  %  PDU_skip_interval  !=  0} 


if  ;TRACE;  cout  <■ 
return  (TRUE) ; 


returning."  <<  endl; 


else  if  (TRACE)  cout  <■■    ",    sending  DIS  PDU."  <<  endl; 
p:"evious_clock  =  current_clock ; 

//  see  test_it.c  ('client'  program)  in  DIS  library  for  examples; 

//      In  'elsie/workB/macedoni /net /mcast /network /s re /test  it . c 


UUV_DI£_:d . address . si te 

UU^v_d:  E_id  .  address  .  host 
'J^JV_DI  £_id  .  ent  i  ty 


£ITE_IL_NPS;   //  DIS  standard  para  5.3.8.1.1 

(unsigned  short)  1; 
(unsigned  short)  1; 


//  fcice  the  ^define  values  to  the  right  types 

UU'/_LIS_type  .  entity_kind  =  (unsigned  char)  Enti  tyKind_Platf  orm; 

Uirv'_DI£_type  .domain      =  (unsigned  char)  Domain_Subsurf  ace; 

UUV_DI£_type . country     =  (unsigned  short)  USA; 
//  not  yet  defined  in  DIS  header  file 

//  UU^v'_DI£_type .  category    =  (unsigned  char)  Category  _ResearchMiscSub; 

UUV_DIS_type . subcategory  =  (unsigned  char)  0; 

UU"v/'_DI£_type  .  speci  f  ic    =  (unsigned  char)  0; 

Uu''v/_DIS_pdu  =  mall  ocEnti  tyStatePDU  (  )  ; 

//  fill  in  the  parameters  of  an  entity  state  PDU  (most  are  listed  in  pdu.h) 
z  /         this  assumes  there  are  no  articulated  parameters  (add  later)  ««< 

//  Note  all  units  converted  to  SI  when  building  PDU 

//  DIS  ID  and  Type 

UU\'_DIS_pdu->entity_id  =  UUV_DIS_id; 

Uir>y_DIS_pdu->entity_type  =  UUV_DIS_type; 

UUV_DIS_pdu->f orce_id  =  ForceID_Whi te; 

UUV_DIS_pdu->alt_entity_type .extra  =  (Extra)  0x00;   //  unused  extra  parameter 

//    Posture 

UiJV_DIS_pdu->entity_location.x  =   AUV_x    *    METERS_PER_FT 

UUV_DIS_pdu->entity_location.y  =   AUV_y    *   METERS_PER_FT 

UUV_DI£_pdu->entity_location.z  =   AUV_z    *   METERS_PER_FT 

UUV_DIS_pdu->entity_orientation.psi  =    (int)    degrees    (AUV_psi)         %    360 

UUV_DIS_pdu->entity_orientation. theta  =    (int)    degrees    (AUV_theta)    %    360 

UU^/_DIS_pdu->entity_orientation  .phi  =    (int)    degrees    (AUV_phi )         %    360 
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//   Linear    and   angular   velocities  in   body    coordinates/meters   by   DIS   standard 
Uirv^_DI£_pdu->encit:y_velociCy  .X  =' AUV_x_dot    *    METERS_PER_FT; 

UUV_DIS_pdu->entity_velocity .y  =   AUV_y_dot    '    METERS_PER_FT; 

in.TV_riF_i:.du->encity_velocity  .  2  =    AUV_z_dot    *   METERS_PER_FT; 


UUV_il':  f_i;)du- -dead_reckon_parajns  .  angL;lar_velocicy    [0]    - 

(int)    degrees    (AUV_phi_doc )         %    360; 

'-n,TV_DI£_pdu-->dead_reckon_params .  angular_velocity  [1]  = 

line)  degrees  (AUV_theta_dot )  %  360; 

UUV_DI£_pdu-->dead_reckon_paraiDs .  angular_velocity  [2]  = 

(int)  degrees  (AUV_psi_dot )    %  360; 


' .'    n>'    explicit   world   coordinate  accelerations   are  provided  by   dynair.ics  model 
UiJV_:  I£_pdu- >dead_it-ckoii_paranis  .  linear_accel     (Oj  =    0.0, 

'T;n;_:  :c-_pjy_^^(^3(-]_re,;)<on_parains  .  linear_accel    [1]  =0.0, 

UUV_DI£_pdu->dead_reckon_parains .  linear_accel     [2]  =    0.0, 


.■  ■    oebu;:    coae  -. 

//   iJUV_Dl£_pdu->encity_velocity  .X  =0.0 

iro"v'_:':£_pdu--enr  ^ry_veloci  ty  .y  =  0.0 

,  ,    'JU\'_r'I£_pdu-'»ent  ity_veloci  ty  .  2  =  0.0 

U^J''y_r-I£_pdu->dead_reckon_params .  angular_velocity  [0]  =    0; 

■    UUV_I'I£_pdu->dead_reckon_params .  angular_velocity  [1]  =    0; 

/'/   'JUV_DI£_pdu->dead_reckon_params .  angular_velocity  [2]  =    0; 


'TRUE; 

{ 

ecu 
ecu 


CO. 


cut 
out 
r/j  t 
cut 
out 


endl ; 

"DI£   net   write: 


time 


<<  AUV   time   << 


irLr;_DI£_pdu--^enti  ty_Iocation  .x 
UUV_DI£_pdu->entity_location.y 
UlJ\'_DI£_pdu->entity_Iocation .  z 
UL''.'_D  I  £_pdu  ->entity_orientation.phi 
U'JV_DI£_pdu->entity_orientation .  theta 
UUV_DI£_pdu->entity_orientation .psi 


<< 

" 

" 

<< 

•' 

" 

<< 

" 

" 

« 

" 

" 

« 

" 

" 

« 

" 

"  « 

endl 

if     'TRACE; 

I 

cout  -'■'  "World  coordinate  velocities: 


c  ou  t 

ecu  T 

cojt 
cout 
cout 


UU\'_DI£_pdu--.-enti  ty_velocity  .X  <<    ", 

U'A'_r!£_pdu---entity_velocity  .y  <«-    ", 

nij\'_DI£_pdu->enti  ty_velocity  .  z  <--    ', 

ULA/_D I S_pdu - >dead_r  ec  k  o  n_pa  rams.angula  r_ve 1 oc  i  ty    [0 
UW_DI£_pdu->dead_reckon_params  .  angular_velocity    [  1 
Uirv'_r'I£_pdu->dead_reckcn_param3  .anguiar_velocity    [2  [ 
endl ; 


<< 
« 


if  (TRACE) 

( 

cout  «  "World  coordinate  accelerations:  "; 

cout  «  " 1 " ; 

cout  «  UUV_DIS_pdu->dead_reckon_params.linear_accel  [0]  « 

cout  «  UUV_DIS_pdu->dead_reckon_params . linear_accel  [1]  « 

cout  «  UUV_DIS_pdu->dead_reckon_params . linear_accel  [2]  « 

} 


] •  <<  endl ; 


//  what  we  look  like 

UUV_DI£_pdu->entity_appearance  =  AppearanceSubSurf_SmallWake; 
UUV_E'IS_pdu->entity_marking  .  character_set  =  CharSet_ASCII ; 
strncpy  ((char  *)  UUV_DI£_pdu->entity_marking. markings,  "NPS  AUV    ", 

MARKINGS_LEN) ; 
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AmiculatParamsNode  *  APNpCr; 


//  articulated  parameters 
:  bd  rpn 


rudders,  planes,   test  with  execution  level  rpm. 


/■/  iioct-  that  the  function  specification  for  attachArticulatParamsNode 
needed  to  be  correctfcd  to  acceot  (char  ')  in  file  disdefs.h 

if  (APNptr  =  attachArticulatParamsNode  ((char  *)  UUV_DIS_pdu, 

EntityStatePDU_Type) ) 

( 

//  Successful  attach,  fill  in  data.   See  DIS  standard  paragraph  5.3.3 
APNptr->articulat_params . change  =  TRUE;  //  always  active 
APNptr->articulat_params . ID     =  0;     //  articulated  parameter  # 


APNptr->articulat_params .parameter_value 

=  (unsigned  short 

APNptr-->articulat_params .  parameter_value 

=  (unsigned  short 

APNptr-->art  iculat_params  .parameter_value 

=  ( signed  short i 

ArMpt"  r-  -3  rt  iculat_p'araiT'3  .  parameter_value 

=  (signed  short; 

APNpti"->articulat_param.s  .parameter_value 

-  (signed  short) 
A  r/lr.  t  r  -  "--a  r c  1  cu  1  a  t _pa  r  am?  .  pa  rame  ter_va  lue 

=  '.signed  short) 
APNptr-  -art:culat_p5ram3 . parameter_value 

=  ( signed  short ) 
APNptr-  »articulat_ijarams  .parameter_va'lue 

=  (signed  short) 


0]   //  auv  time,  even  seconds 

((int)  AUV_time  %  600); 
1]  //  auv  time  tenths  of  seconds 

(      AUV_time  '  10)  %  10; 
2] 
(int)  degrees  (AUV_delta_rudder ) 

%  3  60) ; 
3  1 

(int)  degrees  (AUV_delta_planes) 

%  3  60) ; 
41 

(int)  AUV_port_rpm     /   10) 

5] 

(int)    fabs    (AUV_port_rpm)    %   10) 
6] 

(int)       AUV_stbd_rpm   /  10) 
7] 
(int)  fabs  (AUV_stbd_rpin)  %  10) 


ei. 

se   ccul 

srticul 

•,  f 

(APNiitr 

'  "(error  attaching  articulated  parameters  node  0:   time,  " 
"rudder,  planes,  rpmi]  "  «  endl; 

:ed  pararrieters  :   Thruster  parameters 

:  attacf-LArticulatParamsNode  ((char  *)  UlJV_DIS_pdu, 

EntityStatePDU_Type) ) 

//  Successful  attach,  fill  m  data.   See  DIS  standard  paragraph  5.3.3 

APNptr->articulat_parains.  change  =  TRUE;  //  always  active 

APHptr->articulat_params. ID     =1;     //  articulated  parameter  # 

APNptr->articulat_params .parameter_value  [0]  -    (int)  AUV_bow_vertical ; 

APNptr->articulat_parains .parameter_value  [1]  =  (int)  AUV_stern_vertical ; 

APNptr->articulat_params  .parameter_value  [2]  =  (int)  AU\'_bow_lateral ; 

APNptr->articulat_params .parameter_value  [3]  ~    (int)  AUV_stern_lateral ; 

APNptr->articulat_params .parameter_value  [4]  =  0; 

APNptr->articuiat_params .parameter_value  [5]  =  0; 

APNptr->articulat_params .parameter_value  [6]  -    0; 

APNptr->articulat_params .parameter_value  [7]  =  0; 
] 

else  cout  <<    "[error  attaching  articulated  parameters  node  1:  thrusters] " 
<•<  endl ; 


//  articulated  parameters:   Sonar  parameters 

if  (APNptr  =  attachArticulatParamsNode  ((char  *)  UUV_DIS_pdu, 

EntityStatePDU_Type) 


1 


//  Successful  attach,  fill  in  data.   See  DIS  standard  paragraph  5.3.3 
APNptr->articulat_params . change  -   TRUE;  //  always  active 
APNptr->articulat_params . ID     =1;     //  articulated  parameter  # 
APNptr->articulat_params .parameter_value  [0] 

=  (signed  short)  {(int)  AUV_ST1000_bearing)  /  10; 
APNptr->articulat_parains  .parameter_value  [1  ] 

=  (signed  short)  {(int)  AUV_ST100  0_bearing)  %  10; 
APNptr->articulat_params .parameter_value  [2 ] 


226 


=  (signed  short) 
I  (int )  (AU\'_ET10n^_range*4  .0-^0  .5)  )  ; 

APNpi:r->arLiculat_params  .paraiTieter_value  [3  ] 

=    (signed   short,  ((int)    AUV_ST1000_strength) ; 

AFNf  t  r-  -art  :c'L^iat_parains  .paraineter_value  [4] 

=    (signed   short)  ((int)    AUV_ST7  2  5_bearing)    /    10; 

APNptr->articuIat_parains  .paraineter_value  [5] 

=  (signed  short)  ((int)  AUV_ST7  2  5_bearing)  %  10; 

APNptr->art iculat_params .parameter_value  [6 ] 

=  (signed  short)  ( ( int ) (AUV_ST725_range*4 . 0+0 . 5 ) ) ; 

APNptr->articuIat_params . parameter_value  [7 ] 

=  (signed  short)  (lint)  AUV_ST725_strength) ; 
) 

el^^  ccur  ■■•  ";-^rror  actaching  articulated  parameters  node  2:   sonar]" 
----'  endi  ; 

//  Identify  dead  reckoning  algorithm:   world  coordinates,  accelerations  zero 
'■;  (Algorithm  3) 

UUV_LJl£_pdu->dead_reckon_params.  algorithm  =  DRAlgo_DRM_RPW; 

.'■  /  send  out  the  multicast  PDU 

in"   result  =  net_wr:te  ((char  *!  UU\'_DI£_pdu ,  Enti  tyStatePDU_Type)  ; 

it       (result  ==  FALSE) 
{ 

cout  «  "DI£_net_write  ()  failed"  <<  endl ; 

else  :f  {TFJ\CE)    cout  <<  "DI£_net_write  ()  successful,  returning"  <<  endl; 

ireeE:.ti  tyStatePDU  {Uirv'_DI£_pdu )  ;   //  essential  to  prevent  memory  leak 

//  articulated  parameters  are  also  freed 

return  result;   /,'  end  of  DIS_net_write  () 
/, // 

void  DISNetworKedP.igidBody :  :  DI£_net_close  (i 

DI£:_port_open    =    FALSE; 

net_CiOse    i ; ; 
I 
// // 

void   ZI  SlJetworkedP.igid&ody  :  :    DISNecworkedRigidBody_ini  t  ialize    () 

Pi  .,•:  :jEr  d>_i:ii :  lal  1  ze    (,;  /  /    inherited  method 

time_of_]ast_PDU   =    0.0; 

; 

// // 

void  DISNetworKedRigidBody : :  set_time_of_last_PDU 

(const  double  new_time_of_last_PDU) 
i 

tim.e_of_last_PDU  =  new_time_of_last_PDU; 

) 

// // 

void  DISNetworkedRigidBody : :  set_ttl   (int    new_ttl) 
{ 

cout  «  "DISNetworkedRigidBody : :set_ttl :    old  ttl   =  •  «  (int)  ttl; 

if  (new_ttl  >  16) 
{ 

cout  «  endl ; 

cout  <-^    "A  large  ttl  value  may  impact  people  outside  your  network  " 
<<  endi ; 

cout  «  "and  even  around  the  world!"  <<  endl; 
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cout  <<  "Please  confirm  by  entering  the  new  time  to  live  (ttl)  " 
<<-  "value  ("  «r<  new_ttl  <<■'  ")  yourself:   "  ; 

cin   »  new_ttl; 

ccut  •-  endi; 
( 
if  ■■  'r,ew_ttl  <  1)  II  {new_ttl  >  255)) 

cout  <<  end!  ; 

cout  <-  "Time  to  live  (ttl)  value  out  of  range  (1..255),  ignored." 

<<•  endl  ; 
ret  urn; 
) 

ttl  =  (u_ch5r)  new_ttl; 

cout  <<  endl ; 

cout  <<  "                                new  ttl    =  " 
<<  (int)  ttl  <<  endl ; 
1 
/  // 

'■"id  'ilrNetworkedRigidEody :  :  set_qroup   (char  *  new_group) 
i 

cout  <<    "DISNetworkedRigidBody : : set_group :  old  group  -    '    <<   group; 

bzerc   'group,   20  i ; 
scrcry  (group,  new_group); 

cout  -  •■  eridi  ; 

ccut  ••-'  "  new  group  =  "  <<  group  «  endl; 

) 

// // 

void  DISNetworkedRigidBody::  set_pcrt   (char  *  new_port) 

cout  <•'■  "DISNetworkedRigidBody ::  set_port  r   old  port   =  "  <<  port; 

b:^:-:::   (port,    6.  ; 
strcpy  (port,  new_port ) ; 

cout  -•''  end!  ; 

cout  ---'  "  new  port   =  "  <<  port  <<  endl; 

// // 

#end:f  //  DISNETWORKEDRIGIDBODY  C 
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RigidBody .C 
Don  Brutzman 
18  October  94 

Irix  5.2 

ANSI  C** 

irix-  make   dynatest 
irix--   CC   RigidBody.C   -Im 


-g  +w 


-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 
*w  ==  Warn  about  all  questionable  constructs. 

Rigid  body  class  specification. 

Note:  world  coordinate  system,  NOT  body  coordinate  system, 
thus  Euler  angles  are  used. 

Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Fu,  K.S.,  Gonzalez,  R.C.,  and  Lee,  C.S.G., 

_Robotics:   Control,  Sensing,  Vision  and  Intelligence_, 

McGraw-Hill  Inc.,  NY,  1987." 

Cooke,  J.C,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 

"NPSNET:  Flight  Simulation  Dynamic  Modeling  using 
Quaternions,"  _PRE£ENCE_,  vol.  1  no.  4,  Fall  1992, 
pp.  4C14-42C. 

Cooke,  Joseph  C,  "NPSNET:   Flight  Simulation  Dynamic 
Modeling  using  Quaternions,"  masters  thesis.  Naval 
Postgraduate  School,  December  1993. 


''!  i  :  I :'  I  I  i  :!!■'!  I  !  I  !  1 1 1  i  I  I  !  1 1 1 1 1 II 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

♦  ifndef  P.IGiri&0:'Y_C 

idefine  RIGIDBCDY_C    //  prevent  errors  if  multiple  #includes  present 

#include  "H.matrix.C" 

r\h.-;-s   P.igidBod'j' 

private  : 

//  member  data  fields 

double    time_of_posture; 


double 
double 
double 
double 
double 
double 


x_dot 
y_dot 
z_dot 

phi_dot 
theta_dot 

psi_dot; 


//  linear  velocity  along  x  axis 

//  linear  velocity  along  y  axis 

//  linear  velocity  along  z  axis 

//  angular  velocity  about  x  axis 

//  angular  velocity  about  y  axis 

//  angular  velocity  about  z  axis 


pUTjilC  : 
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Hrr:3i:i'ix        hiTiatrix; 

//   constructors   and  destructor 

R: JidEody 
RigidBody 

Rig:dBody 

RigidBody 


//   need   to  access  print   &   set    routines 

()  ; 

(const  Hmatrix   initialhinatrix)  ; 

(const  Hmatrix  initialhmatrix,  const  double  t) 

(const  double  x, 

const  double  y, 

const  double  z, 

const  double  phi, 

const  double  theta, 

const  double  psi, 

const  double  t ) ; 


-RigidBody 
■  opt^rar  cr.? 
R: pidBcdyi 


() 


{    /*  null  bod:^'  V  ) 


operator  = 


:rien 


ostream  h.    operator 


(const  RigidBody   S<rb_rhs); 
(ostreams,  out,  RigidBodyS.  rb)  ; 


inspection  methods 

vc.id  print_rigidbody 


(}    const; 


HjTiatrix  n]T,atrix_value  ( 

double  time_of_posture_value       ( 

douL'lc  x_value  ( 

double  y_value  ( 

doucle  z_value  ( 

dc^b^e  pni_vaiue  ( 

dou&le  theta_varue  ( 

double  psi_value  ( 

double  x_dct_value  ( 

double  y_dot_value  ( 

double  z_dot_value  ( 

double  phi_dot_value  ( 

double  theta_dot_value  ( 

double  psi_dot_value  ( 


const; 
const; 

const 
const 
const 

const 
const 
const 

const 
const 
const 
const 
const 
const 


//  assignment 


//  modi  tying  methods 


vcid 
void 


void 


void 


void 


RiqidBod"y_ini  ti  al  ize 
set  velocities 


0  ; 

(double   new_x_dot, 
double   new_y_dot, 
double  new_z_dot, 
double   new_phi_dot, 
double  new_theta_doc, 
double  new_psi_dot) ; 


set_linear_veloci ties   (double  new_x_dot, 

double  new_y_dot, 
double  new_z_dot) ; 

set_angular_velocities  (double  new_phi_dot, 

double  new_theta_dot, 
double   new_psi_dot) ; 


set_time_of_posture 


(double  new_time_of_posture) ; 
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void 


updace_Hinatrix 


(double  delta_c: 


I  ; 

I  ■  t  s  i ,,  1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11  n  1 1 1 1 1 1 1 1 


II 

' :    constructor  methods 
// 


RigidBody::  RigidBody 


//  default  constructor 


rjnatrrx.  set_identi  ty    () 

t  i  nife_o  f  _p  c.  s  t  u  r  e   =    r; .  0  ; 

x_dot  -    O.U; 

y_dot  =    O.C 

2_dot  =    0.0, 

phl_dot  =    0.0; 

theta_dot  =    0.0; 

ps.i_dot  =    0.0, 


■// 


JBodi' 


RigidBody    (const    Hrnatrix   initialhmatrix) 


hjTiatrix   =  Hmatrix  (ini tialhrnatrix)  ; 

tinie_of_posture   =  0.0, 

x_dot  =  0.0, 

y_dot  =  0.0, 

z_dot  =  0.0, 

phi_dot  -  0.0, 

rheta_dot  =  C. 

psi_dot  =  0.0, 


II- 


I  i 


RigidBody::    RigidBody    (const   Hmatrix   initialhmatrix,    const   double   t; 

hiTiatrix    =    Hmatrix  (  ini  tialhmiat  rix  )  ; 

time_of_pcst ure  =  t; 

x_dot       =  0 . 0, 

y_dct       =  0.0; 

z_dot       =  CO; 

ph:_dot       =  CO, 

theta_dot       =  0.0, 

ps._dot       =  CO, 


•// 


Rigj.dBody  :  :  RigidBody  (const  double  x, 

const  double  y , 

const  double  z, 

const  double  phi, 

const  double  theta, 

const  double  psi, 

const  double  t) 


{ 


hmatrix         =  Hmatrix  (phi,  theta,  psi,  x,  y,  z) ; 
time_of_posture  -   t; 
X  dot       =  0.0; 


y_dot 

=  0.0 

z  dot 

=  0.0 

phi_dot 

=  0.0 

theta  dot 

=  0.0 

psi_dot 

=  0.0 
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// // 

RigidBodyS;  RigidBody::  operator  =  (const  RigidBody  £<rb_rhs)     //  assignment 

I 

rjiiaciix  =  rb_rhs  .hmatrix; 
tiIne_of_posture  =  rb_rhs . time_of_posture; 
x_dot  -  rb_rhs .x_dot ; 
y_dot  =  rb_rhs .y_dot ; 
z_dot  =  rb_rhs . z_dot ; 
phi_dot  =  rb_rhs .phi_dot ; 
theta_dot  =  rb_rhs . theta_dot ; 

psi_dot  =  rb_rhs .psi_dot ; 
return    *this; 

) 

/, // 

osLieaiTiAc    operator   -  <-    (ostreami    out,    RigidBodySt    rb) 

rfc  .hrriatrix  .print_hiriatrix    (  )  ; 

ouc  --■••-'  "tiine_of_posture    =  "  «   rb.  time_of_posture  «  endl; 

out  •  •-  "■  x_dot,  y_dot ,  z_dot,  phi_dot,  theta_dot,  psi_dot>  =  <" 

<<   rb.x_dot   <<-  ",  "  <<   rb.y_dot     «  ",  "  «  rb.z_dot   «  ",  " 
•■  rt.ph:_dot  «  ",  "  <-^  rb.theta_dot  «  ",  "  «  rb.psi_dot  «  ">" 

return  out ; 


/ 


/ ,  inspectici.  metnods 


hr^H.: 


■// 


■// 


vcii   RigidBody::    print_rigidbody    () 
const 

hiriatrix.print_hinatrix    ! )  ;  ; 

ccut    <<    "time_of_posture  =    "      <-<   time_of_posture  «   endl; 

cout   «    "<-   x_dot,    y_dot,    z_dot,    phi_dot,    theta_dot,    psi_dot>   =   <" 

x_dct    <<-    ",     "    <<  y_dot   <<    " ,     "    <<        z_dot   <<    "  ,     " 

pni_dot   -'<    ",    "    «   theta_dot   <<    ",    "    <<  psi_dot   <<    ">"    «  endl; 

) 

// // 

Hmatrix  RigidBody::  hinatrix_value  ()    //  note  this  returns  an  Hmatrix  object, 
const  //      not  a  [4]  [4]  array 

return  hmacrix; 

) 

/ , // 

dcuble  RigidBody::  time_of_posture_value  () 

const 

( 

return    t iine_of_posture; 

) 

// // 

double  RigidBody::  x_value  () 

const 

( 

return  hmatrix .x_value  (); 

} 

// // 

double  RigidBody::  y_value  () 

const 

( 

return  hmatrix .y_value  (); 
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I , // 

double  RigidBody::  z_value  () 

leCJir,  hmat:!  IX  .  z_value  ( )  ; 

) 

// // 

double    RigidBody::    phi_value    () 
const 

ret'jrn  hmaciix  .phi_value  I  )  ; 
'l  I // 

double   RigidBody::    rhei:a_value    () 

const 

1 

return  hmat rix . theta_val ue  ( ) ; 

// // 

double  RigidBody::  psi_value  (; 

const 

{ 

return  hmatrix . psi_value  ( ) ; 
} 
'/I // 

double  RigidBody::  x_dot_value  () 
const 

return  x_dot ; 
// // 

double  RigidBody::  y_dcc_value  (i 

return  y_dc.t  ; 
// // 

double  RigidBody::  z_dot_value  () 

C  C  I*!  3 1 
; 

return  z_dot ; 
// // 

double  RigidBody::  phi_dot_value  () 

const 

{ 

return  phi_dot ; 

) 

// // 

double  RigidBody::  theta_dot_value  () 

const 

( 

return  theta_dot; 

) 

// // 

double  RigidBody::  psi_dot_value  {) 
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coni 


return  psi_doc; 


■■   rr:odlf\'ina  niethod: 


/  / 


void   RiaidBody::    Rigid&ody_initiali ze    {) 
I 

hmatrix. set_identity    (); 

cime_of_posCure   =    C.O, 

x_dot  =    0.0, 

y_dot:  =    C.O, 

2_dot:  =    0.0, 

phi_dot  =    0.0 

cheta_dot  =    0.0; 

psi_dot  =    0.0, 


■// 


void   RicjidBody: 


veloci ties 


(double  new_x_dot , 
double  new_y_dot , 
double  new_z_dot, 
double  new_phi_dot , 
double  new_theta_dot , 
double  new_psi_dot) 


X_dC!L 

= 

;-iew_x_dot  ,- 

y_ac: 

= 

Vi'zVJ_j-_ZiOZ  ; 

r  act; 

= 

r)ew_r_dot  ; 

pni_aoc 

= 

new_phi_dot; 

tneta  act 

= 

new  theta  dot 

ps._dac 

= 

new_ps:_dot ,- 

•// 


vo:c  RiuidBody 


set_linear_veloci  ties 

(double  new_x_dot, 
double  new_y_dot, 
double  new  z  dot; 


x_dct  =  new_x_dot 
y_dot  =  new_^'_dot 
z  dot  =  new  z  dot 


//■ 


■// 


void  RigidBody::  set_angular_veloci ties 

(double  new_phi_dot, 
double  new_theta_dot 
double  new_psi_dot) 
{ 

phi_dot  -    new_phi_dot ; 

theta_dot  =  new_theta_dot ; 

psi_dot  =  new_psi_dot ; 


//- 


•// 


void   RiaidBody::    set_time_of_posture    (double   new_tiiTie_of_posture) 
( 

tinie_of_posture   =    new_time_of_posture; 

} 

// // 


void  RigidBody::  update_Hmatrix 
( 


(double  delta_t) 
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hmatrix . incremental_rotation     (   phi_dot ,  theca_dot,  psi_dot,    delta_t); 

hmacrix . incremencal_translation  (Vector3D  (  x_doc,  y_dot ,  z_dot),  delta_c); 

1 

II // 

«-:-i.-3:  f   '  RIGIDBOE'Y  C 
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I.  Hmatrix.C  Homogeneous  Transform  Matrix  Mathematics 

i  1 1  i  i  I!  1 1 : 1 J 1 1  i ,  1 1 ;  1 1 1  n  I / 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 ! 1 11 1 1 1  i  11 11 1 1 1 1  n  1 1 1 1 1 1 

Program:  Hmatrix.C 

Author:  Don  Erutzman 

Revised:  12  October  94 

Systemi:  Ins 

Compiler:  ANSI  C++ 

Compi  lat  ioi; :      irix>  make  dynatest 

irix>  CC  Hmatrix.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  Che  link  phase. 

-rw  ==  Warn  about  all  questionable  constructs. 

Description:      Homogenous  transform  matrix  class  specification 
All  angle  param.eter  values  are  in  radians 

rotations    are  in  world  coordinate  system 
translation?  are  in  world  coordinate  system 

Ajviscii:        I r .  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

References:       Fu ,  K.S.,  Gonzalez,  R.C..  and  Lee,  C.S.G., 

_Pobctics:   Control,  Sensing,  Vision  and  Intelligence_, 
McGraw-Hill  Inc.,  NY,  1987. 

Ccoke,  J.C,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 
"NPSNET:   Flight  Simulation  Dynamic  Modeling  using 
Quaternions,"  _PRESENCE_,  vol.  1  no.  4,  Fall  1992, 
pp.  404-420. 

Cooke,  Joseph  C,  "NPSNET:   Flight  Simulation  Dynamic 
Modeling  using  Quaternions,"  masters  thesis,  Naval 
Postgraduate  School,  December  1993. 

*  '' 

I'l  i  1 1 1 1 !!  I  i  1 1 !  1 1 1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

#ifndef  HMATRIX_C 

#define  HMATR:x_C    //  prevent  errors  if  multiple  ^includes  present 

#include  "Quaternion. C" 

class  Hmatrix 

( 

private : 

//  member  data  fields 

double   htmatrix  [4][4]; 
public : 
//  constructors  and  destructor 

Hmatrix  (  )  ; 

Hmatrix  (const  double       x, 

const  double       y. 
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rin-a:  rix 


Hinatrix 

Hir.arrix 
-Hir.at  lix 

Hr'arr^xi 
H.T.a::rlxSr 
Hir.acrixSi 


const  double 
const  double 
const  double 
const  double 

(const  Vector3D 
const  double 
const  double 
const  double 

(const  Vector3D 

const  VectorBD 


z , 

phi  , 
theta, 
psi  )  ; 

position_Vector3D, 
phi , 
theta, 
psi)  ; 

position_Vector3D, 
eulerangles3D) ; 


(const  Vector3D   position_Vector3D, 
const  Quaternion  posture) ; 


(const  Hmatrixi 
() 


input_hmatrix) ; 

i  / *  nu 1 1  body  * /  } 


operator  =  (const  Hmatrix  5<h_rhs) 
operator  '  (const  Hmatrix  &h_rhs) 
operator  *     (const  VectorSD  &v_rhs) 


//  assignment 

//  Hmatrix  multiply 

//  Vector3D  multiply 


friend  ostreami.  operator  <<  (ostream&  os,  HmatrixSi  h); 
'/    inspection  metnods 

void       print_hmatrix  ()  const; 


double 

x_value 

0 

const; 

dounle 

y_value 

0 

const ; 

do  u  i:  1  c 

z_valufc 

{) 

const; 

60CL-<- 

P 

n-_valufe 

;  ) 

const ; 

dcu  L-i.  e 

z 

'■ieta_value 

(1 

const ; 

30 -Dae 

P 

si_value 

() 

const; 

Cuaterr 

- 

on 

quaternion. 

_value 
(  i 

const; 

V  e  c  t  o  r  J 

L 

position 

(i 

const ; 

V-  :>: '  il 

L' 

carr.er  a 

(  ) 

const ; 

doucie 

scale 

0 

const; 

double 

element 

(const  ir 

//  modifying  methods 


void 


rotate 


ow,  //  accessor  returns  lvalue 

onst  int  column)  const;  //    for  row/column  [1..4] 


(const  double   phi, 
const  double   theta, 
const  double   psi); 


void 

rotate 

void 

rotate_x 

void 

rotate_y 

void 

rotate_z 

void 


(const  Vector3D  rotation3D) ; 

(const  double   phi); 
(const  double   theta); 
(const  double   psi); 


incremental_rotation 

(const  double  phi_dot, 
const  double  theta_dot, 
const  double  psi_dot, 
const  double  delta  t   ) 
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void       translace     (const  double   delta_x, 

const  double   delta_y, 
const  double   delta_z); 

void       translate      (const  VectorBD  translation3D) ; 

void       increrTiental_translation 

(const  double   x_dot ,   const  double  y_dot, 
const  double    z_dot ,   const  double  delta_t); 

void  increiTiental_translation 

(const  Vector3D  velocitiesBD, 
const  double   delta_t.); 

void       set_identity   (); 

void       set_posture    (const  double  phi, 

const  double  theta, 
const  double  psij; 

vc:d       set_picsit ion   (const  Vector3D  translation3D)  ; 

void       set_camera     (const  Vector3D  transIation3D) ; 
vcid       move_camera    (const  Vector3D  translation3D) ; 

void       set_all_scales (const  double  scale_x, 

const  double  scale_y, 

const  double  scale_z, 

const  double  scale_global ) ; 
void       set_3Cale      (const  double  scaie_global ) ; 

i  ; 

1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 / 1 1 n  1 1 1 1 1 1 II 1 1 

II    rotation  matrix  conventions:   Cooke  et  al .  Figure  10 

//  rotation  matrix 

^define  al  htmatrix ( 0 ) [ 0] 

^define  a2  htmatrix [1 ][ 0] 

*define  a3  htmatrix [2 ][ 0] 

♦define  bl  htmatrix [0 ][ 1 ] 

#define  b2  htmatrix [ 1] [ 1 ] 

#define  b3  htmatrix [2 ]  i 1 ] 

♦define  cl  htmatrix [0 ][ 2 ] 

♦define  c2  htmatrix [ 1 ] [2 ] 

♦define  c3  htma trix [ 2 ]  i 2 ) 

//  position  translation  vector 
♦define  p_x    htmatrix [0 ]( 3 ] 
♦define  p_y    htmatrix [ 1 1 ( 3 ] 
♦define  p_z    htmatrix [2 ][ 3 ] 

//  camera  perspective  transformation 
♦define  c_x    htmatrix [3 ][ 0] 
♦define  c_y    htmatrix [3 ]( 1 ] 
♦define  c_z    htmatrix [3 ] [2 ] 

//  global  scale  coefficient 
♦define  hscale  htmatrix 1 3 ] 1 3 ] 

// // 

//  constructor  methods 

// // 

Hmatrix::  Hmatrix  ()  //  default  constructor 
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set_idencity  (  i  ; 

1 

// // 

Hmacrix::  Hmatrix  (const  double  x, 
const  double  y, 
const  double  z, 
const  double  phi , 
const  double  theta, 
const  double  psi)  //  constructor 
I 

set_identity  ; ) ; 
rotate        (phi,  theta,  psi); 
trar.rlate     !Ve7tor3E'  fx,  y,  z ,  ;  ; 
} 
/ // 

Hrr.atrix::  Hrriatrix  (const  VectorSD   posi tion_Vector3D, 
const  double     phi , 
const  double    theta, 
const  double     psi ) 

set_identi ty  ; ) ; 

rotate        (phi,  theta,  psii; 

tiar.siate     iposi  tion_Vector3D.  ; 

// 

KiT.atrix::  HiTidtriX  (const  Vector3D  position_Vector3D, 
const  Vector3D  eulerangles3D) 

set_:dentity  { ) ; 

rotate       (euierangles3D) ; 

translate     (pcsl tion_Vector3D; ; 
I 
// // 

Hn;atr-X::  Hrnatrix  '.const  Vectcr3D  posit  ior._Vector3D,  const  Quaternion  posture) 

3e:_:denti ty  : ; ; 

rctatt       (posture . euler_angles  ()); 

trar..-late     (posi  tion_Vector3D)  ; 

) 

,  / // 

H.-;.ati.x::    Hrnatrix    (const    Hmatiixt^    input_hiTiatrix) 

for    ;int    row   =    0;    row   ■--    3;    row-f-rj 
{ 

for    (int    ccluinn   =    0;    column   -:-    3;    coluinn  +  +) 

1 

htmatrix  [ row] [column]  -    input_hmatrix .element  (row+l,  column+1); 

) 

) 

) 

// // 

//  operators 

// // 

Hmatrixs.  Hmatrix::  operator  =  (const  Hmatrix  &h_rhs)       //  assignment 
{ 

for  (int  row  =  0;  row  <=  3;  row+4) 
( 

for  (int  column  =  0;  column  <=  3;  column++) 
I 

htmatrix  [ row] [column]  =  h_rhs . htmatrix  [row] ( column] ; 
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} 

5 

return    *this; 
} 


•// 


HiTiatrixi    Hmatrix::    operator    *    (const   Hrriatrix   £(h_rhs)         //   Hmatrix  multiplication 
{ 

static  Hmatrix  hresult  =  Hmatrix  (); 

int  row,  column,  index; 

for  (row  =  0;  row  <=  3;  row++) 

for  (column  =  0;  column  <=  3;  column++) 
{ 

hresult . htmatrix  [ row] [column]  =  0.0; 
} 
} 

for  (row  =  0;  row  <=  3;  row++) 

for  ; column  =  0;  column  <=    3;  column  +  +) 

for  (index  =  0;  index  <=  3;  index-f  +  ) 
i 

hresul t . htmatrix  [row] [column]  =  hresult .htmatrix  [row] [column]  + 
htmatrix  [row] [index]   *  h_rhs .htmatrix  [ index] [column] ; 
} 

} 

t  ' 

return  hresult; 
) 
/  // 

Hmatrixc.  Kmatrix::  operator  *  iconst  Vector3D  &v_rhs)      //  Vector3D  multiply 

static  Hmatrix  hresult  =  Hmatrix  (); 

mt  row,    column; 

double   position_vector    [4]; 

posi tion_vector  [0]  =   v_rhs  [1] 

pcs^ ticn_vector  [ij  =   v_rhs  [2] 

posi tion_vector  [2]  =   v_rhs  [3] 

posi tion_vec tor  [3]  =    1.0; 

for  (row  =  0;  row  <=  3;  row+j-) 
( 

for  (column  =  0;  column  <=  3;  columri  +  +) 

( 

hresult .htmatrix    [row] [column]    =    0.0; 

) 
) 

for    (row   =    0;    row   <=    3;    row++) 
{ 

for  (column  =  0;  column  <=  3;  column++) 
{ 

hresul t . htmatrix  [ row] [column]  = 

htmatrix  [ row] [column]  *  position_vector  [column]; 
} 
) 
return  hresult; 

) 

// // 
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oscream^  operator  «    (oscreamSt  out,  HiDatrix&  h) 
{ 

mr  row; 

for  ,row  =  0;  row  <=  3;  row+-^i 
( 

out   <>--  endl  <<  "  [  " 

<<   h.htmatrix  |row)[0]  <<  ",  "  «  h.htmatrix  [row][l]  <<  ",  " 
<<  h.htmatrix  [row] [2]  «  ",  "  «   h.htmatrix  [row] [3]  <<  "]"; 
) 

out  • ■  endi ; 
return  i  out  j ; 
} 
'.  f // 

//  inspection  methods 

// // 

void  Hn-.amx::  print_hiriat rix  f) 
const 

int  iOW; 

for  (row  =  0;  row  ^=  3;  row+*) 

ccut  ■' <    endi  <<    "  [  " 

<■    htmatrix  [rowj[01  «    ",    "    «   htmatrix  lrow][l]  <<  ",  " 
<■-    htiTiatrix  !rowj  (2)  -'<  ",  "  <<  htmatrix  [row]  [3]  <<  "  ]  "  ; 

) 

;cut  •  ■  endl ; 

// // 

double  H.T,atriX::  phi_value  ()  //  Cooke  (8.17) 

const 

{ 

double  theta   =  theta_value  ( I ; 

double  res'L-l  t  ; 

if  '(theta  ==  radians  (  90.0;;  II  {theta  ==  radians  (-90.0))) 

cout  ••••  "phi_value  ()  warning:  theta  ==  "  <<  cheta  <<  endl; 
I 

result  =  acos  'arcclamp  (c3  /  cos  (theta)))  *  sign  (b3); 
ret-rn  rej-^lt; 
1 


double  Hmatrix::  theta_value  ()  //  Coo)<e  (8.14) 

const 

return  asin  (arcclamp  (-a3)); 

// // 

double   Hmatrix::    psi_value    ()  //   Coolie    (8.16) 

const 

{ 

double  theta  -    theta_value  (;; 

double  result; 

if  ((theta  ==  radians  (  90.0))  II  (theta  ==  radians  (-90.0))) 
{ 

cout  <<  "psi_value  ()  warning:  theta  ==  "  <<  theta  «  endl; 
) 


241 


result    =   acos    (arcclamp    (al    /   cos    (Cheta_value    ())))    *    sign    (a2) 
return   result; 


double  Hiriatrix::    x_value    () 

const 

{ 

return  p_x; 


II- 


■II 


■II 


double   Hmatrix::    y_value    {) 

const 

1 

return  p_y ; 


// 

double  Hmatrix: 
const 


■// 


z_value    ( 


return   p_2 


Quaternion   Hiriacrix::    guaternion_value    ()        » 
const 

Quaternion  qresult  =  Quaternion  ! ) ; 

dcuole     qO,  ql,  q2 ,  q3 ; 

qC  =  (0.5;  *  sqrt  (al  +  b2  +  c3  +  1); 
ql  =  0.0, 
q2  =  0.0 
q:  =  0.0 

//  .,•.-.-.<.-.-,•<-  Shoemake  p.  253,  FUNDA  or  p. 41  cooke  <««««< 

return  qresult; 


■// 


//■ 


VectorBD  Hmatrix::  position  !) 

const 

( 

return  Vector3D  (p_x,  p_y,  p_z ) ; 
) 


II- 


■II 


■II 


Vector3D   Hiriatrix::    camera    () 

const 

I 

return  VectorBD  (c_x,  c_y,  c_z : 
) 


II- 


■II 


double  Hmatrix::  scale 
const 
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recurn  hscaie; 
} 

,  I // 

double  Hmatrix::  element  (const  int  row,       //  accessor  returns  value 

const  int  column)    //  with  row/column  [1..4] 
c  on  3 1 
i 

if  (  (row  v;  1 )  II  { row  >  4 )  I  l  (column  <  1)  II  (column  >  4 )  ) 

cout  <<  "Warning;  Hmatrix . element  ("  «  row  <<  " ,  "  <<  column  << 
"j  IS  an  invalid  accessor  (only  1..4  allowed),  "  << 
"  returning  value  of  0.0"  «   endl ; 

static  double  dummy _value  =  0.0; 
return        dumimy_value; 

returr.  htmatrix  i  row- 1 )  (  column-1 1  ; 

// // 

1 1    modif'.'inc   riiethods 

// // 

void  Hmatrix::  rotate  (const  double  phi,  const  double  theta,  const  double  psi ) 

double  hrotaticn  i3]|3]; 
double  nresu^it    (jj  [  3  ]  ; 

int  row,  column,  index;     //  reference:   Coo)<e  et  al  .  Figure  10 

double  sinphi    -   sin  (phi); 

double  cosphi    =  cos  (phi); 

double  smtheta  =  sin  (theta); 

double  costheta  =  cos  (theta); 

double  sinpsi    -    sm  (psi); 

double  cospsi    =  cos  (psi i ; 

hrotation  (OJlQl  =  costheta  *  cospsi;  //  al 

hrotation  illiOj  =  costheta  *  sinpsi;  //  a2 

hrotatioij  [2]  [Oj  =  -  smtheta;  //  a3 

nrotation  |0] [1]  =  sinphi  *  sintheta  *  cospsi  -  cosphi  *  sinpsi;  //  bl 

hrotation  [1] [Ij  =  sinphi  *  sintheta  *  sinpsi  +  cosphi  *  cospsi;  //  b2 

hrotation  i2]llj  =  smpni  '  costheta;  //  b3 

hrotation  [0] [2]  =  cosphi  *  sintheta  *  cospsi  +  sinphi  *  sinpsi;  //  cl 

hrotation  |1](2)  -  cosphi  *  sintheta  *  sinpsi  -  sinphi  *  cospsi;  //  c2 

hrotation  [2) [2)  =  cospni  *  costheta;  //  c3 

for  (row  =  0;  row  <=  2;  row+f) 
( 

for  (column  =  0;  column  <-   2;  column++) 

i 

hresult  [row] [column]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 
{ 

hresult  [row] [ column]  =  hresult  [row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [ index] [column] ; 
} 
} 
} 
for  (row  =  0;  row  <=  2;  row++) 

{ 

for  (column  =  0;  column  <=  2;  column++) 
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hcmatrix  [  rowjMcolumn]  =  hresult  I  row]  [column]  ; 
} 
) 
) 
,, // 

void  Hmacrix::  rotate  (const  Vector3D  rotation3D) 
{ 

rotate  (rotation3D  [1],  rotation3D  ( 2 ] , rotat ion3D  [3]);   //  note  not  0  12 
} 
// // 

void  Hmatrix::  rotate_x  (const  double  phi) 

i 

double  hrotation  i3][3]; 
double  hresult    [3]  [3]; 

inc  row,  column,  index; 

for  :  row  =  0;  row  <=  2;  row+  +  ) 

for  (column  =  0;  colum.n  <=  2;  column-+) 
{ 

if   (row  ==  column)  hrotation  [ row) [column]  =  1.0; 

else  hrotation  [rowl [column]  =  0.0; 

1 

hrotation  11] ;1)  =  cos  (phi) 

hrotation  i2i;2j  =  cos  (phi) 

niotation  [2j[l]  =  sin  (phi) 

n rot at  ion  [1]  [2]  =  -  sin  (phi) 

for  ircw  =  0;  row  <=  2;  row++) 
{ 

for  (column  =  0;  column  <=  2;  column++ ) 

t 

hresult  i row)  1  column)  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 
( 

hresult  [ row j  [ column]  =  hresult  [ row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [index] [column] 


for  (row  =  0;  row  <-    2;  row++) 
( 

for  (column  =  0;  column  <=  2;  column++) 

( 

htmatrix  [row] (column]  =  hresult  [row] [column]; 

) 
} 

// // 

void  Hmatrix::  rotate_y  (const  double  theta) 
( 

double  hrotation  [3] [3]; 

double  hresult    [3] [3] ; 

int  row,  column,  index; 

for  (row  =  0;  row  <=  2;  row++) 
( 

for  (column  =  0;  column  <=  2;  column++) 

{ 

if   (row  ==  column)  hrotation  [row] [column]  =  1.0; 
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else 


hrotation  [row] [column]  =  0.0; 


I 

hrota:;ion  [0]!0]  -  cos  (cheta; 

hrotation  [2] [2]  =  cos  (cheta; 

hrotation  [2][0]  =  -  sin  (theta) 

hrotation  [0][2]  -  sin  (theta) 


for  (row  =  0;  row  <=  2;  row++) 

for  (column  =  0;  column  <=  2;  column++) 
{ 

hresult  [row] [column ]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  mdex  +  f) 
{ 

hresult  [row] [column]  =  hresult  [row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [ index] [column] 
} 


for  'row  =  (I;  row  -  =  2;  row+-<-) 

foi  I  column  =  0;  column  ■'=  2;  column^+; 

htmatrix  i row] [column]  -   hresult  [ row] [column] ; 


void  Hmiatrix::  rotate_z  (const  double  psi) 

dou!:.-  hrotation  [3]  [3]; 
double  hresult    [3]  [3]  ; 

int  row,  column,  index; 

for  ; row  =  0;  row  <-    2;  row++l 


for  (column  =  0;  column 


:oiumn-^  + 


if   (row  ==  colum.n   hrotation  [row]  [column]  =  1.0; 
elstr-  hrotation  [row]  [column]  =  0.0; 


i 


hrotation  [0  j  [ 0] 

hrotation  [1 ]  [1] 

hrotation  [1 ] [0] 

hrotation  i  C  ;  [  1  ] 


cos  (psi I 

cos  (psi) 

s 1 n  (psi; 

sin  (psi; 


for  (row  =  0;  row  •;=  2;  row-  +  j 

for  (column  =  0;  column  <=  2;  column++) 
{ 

hresult  [ row] [column]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 
{ 

hresult  [row] [column]  -   hresult  [row] [column]  + 

htmatrix  [row] [index]  »  hrotation  [index] [column] 
} 
) 


for  (row  =  0;  row  <=  2;  row++) 
{ 

for  (column  -    0;  column  <=  2;  column++) 
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hciTiatrix  [row]  [column)  =  hresult  [row]  [column]; 


void  KiTiacrix::  incremental_rocation  (const  double   phi_dot, 

const  double  theta_dot, 
const  double  psi_dot, 
const  double  delta_t   ) 

( 

rotate  (phi_dot  *  delta_t,  theta_dot  *  delta_t,  psi_dot  *  delta_t) 


■// 


.d  Hmatrix::  translate  (const  double  delta_x, 

const  double  delta_y, 
const  double  delta  z) 


■// 


p_x    +  =   de 1 1  a_x 

p^    +=    delta_y 
p_z    +=   delta_z 


) 
//■ 


■// 


void  Rrr^atrix::  translate  (const  Vector3D  translation3D) 


[._'.<    *=    t  rai'isi at  ion^L    [1 
p_y    -►=    translation3D    [2, 
p_2    *=    translation3D    [3 


) 


•// 


void   .-Ln;at  ri  X  :  :    increnier;ta^ 


:ranslation  (const  double  x_dot, 

const  double  y_dot, 

const  double  z_dot, 

const  double  delta_t) 


p_x  +=  x_dot  *  delta_t 
p_j'  +=  y_dot  *  delta_t 
p_z  -'=  z_dot  *  delta_t 


void  HiTiatrix::  incremental_translation  (const  Vector3D  veloci ties3D, 

const  double   delta_t) 
{ 

p_x  +=  velocities3D  [1]  *  delta_t 

p_y  +=  velocities3D  [2]  *  delta_t, 

p_2  *■-   veiocities3D  [3]  *  delta_t 
} 


■// 


//■ 


void  Hmatrix::  set_identity  {)  //  default  constructorset  to  identity  matrix 
( 

for  (int  row  =  0;  row  <=  3;  row++) 
{ 

for  (int  column  =  0;  column  <=  3;  column++) 
( 

if    (row  ==  column)  htmatrix  [ row] [column]  =  1.0; 
else  htmatrix  [row] [column]  =  0.0; 

} 


■// 


} 
//■ 


■// 
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void  Hmatrix::  set_po3ture 


(const  double  phi, 
const  double  theta, 
const  double  psi) 


set_identi ty  ( ) ; 

rotate  (phi,  theta,  psi); 


II- 


■II 


void  Hmatrix::  set_position   (const  Vector3D  translation3D) 

p_x  -  transl3tion3D  [1] 
p_y  =  translation3D  [2] 
p_z  =  translationjD  [3] 


/ 1  ■ 


■II 


void  Kiratrix: 
i 


set  camera 


(const  Vector3D  translaCion3D) 


c_x  -  translaticr.BD  [1] 
c_y  =  transiationSD  [2] 
c  z  =  tran?lat  icnSE'  [3] 


■// 


void  Pxatrix::  mcve_caiTiera 

c_x  -=  t ransl ation3D  ;i! 
c_i'  -^=  t  ran3lation3D  [2] 
c_z  *-    translation3D  [3] 

// 


(const  Vector3D  translation3D^ 


•// 


void  Hmatrix 


al 
fc2 


hscale 


set  all  scales 


scale_x; 
scale_y; 
scale_z; 
scale_global ; 


[const  douDle  scale_x, 
const  double  scale_y, 
const  double  scal€_z, 
const  double  scale_global ; 


//- 


// 


void  HiT.atrix::  set_scale  (const  double  scale_global ) 

hscale  •*=  scale_global  ; 
i 

1 1 1 1 1 1 1 1 !  1 1 1 1 1 1 !  1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
I!    restore  defines 

#undef  al 
ttundef  a2 
#undef  a 3 
#undef  bl 
#undef  b2 
#undef  b3 
#undef  cl 
ttundef  c2 
«undef  c3 
#undef  p_x 
#undef  p_y 
#undef  p_z 
«undef  c_x 
♦undef  c_y 
#undef  c  z 
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#jndef    hscale 

1 1 1 !  I  ill  1 1 1 1 1 1 1  / 1 1  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

#eridif  //    #ifndef    HMATRIX   C 
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J.  Quaternion.C  Quaternion  Mathematics 


1 1  i ;  1 1 !  I !  I  j  1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ;  1 1 1 1 1 1 1 1 !  1 1 1 1 1  / 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

Program:  Quaternion.C 

Revisions:  Don  Brutzman 

Revised:  2  9  SEP  94 

System:  Irix 

Comp-ier:  ANSI  C+* 

Comp..iat  ion  :      irix.>  make  dynatest 

irix>  QQ   Quaternion.C  -Im  -c  -g  +w 

-c  =-  Produce  binaries  only,  suppressing  the  link  phase. 
.^w  ==  Warn  about  all  questionable  constructs. 

I'e.?crii.  "  io:. :      C'uaternion  class  specifications  and  implementation 
Ail  angle  parameter  values  are  in  radians. 

Advisors:        Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

References:      Haynes,  Keith,  "Computer  Graphics  Tools  for  the 

Visualization  of  Spacecraft  Dynamics,"  masters  thesis. 
Naval  Postgraduate  School,  December  1993.   Includes 
source  code  used  for  initial  version  of  quaternion. c 

Cooke,  j.C,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 
"NPSNET:   Flight  Simulation  Dynamic  Modeling  using 
Quaternions,"  _PRESENCE_,  vol.  1  no.  4,  Fall  1992, 
pp.  404-420. 

Cooke,  Joseph  C,  "NPSNET:   Flight  Simulation  Dynamic 
Modeling  using  Quaternions,"  masters  thesis.  Naval 
Postgraduate  School,  December  1993. 

Chou,  Jack  C.K.,  "Quaternion  Kinematic  and  Dynamic 
Differential  Equations, "  IEEE  Transactions  on  Robotics 
and  Automation,  vol.  8  no .  1,  February  1992,  pp. 53-64. 

Funda,  Janez,  Taylor,  Russell,  and  Paul,  Richard  P., 
"On  Homogenous  Transforms,  Quaternions,  and  Computational 
Efficiency,"  IEEE  Transactions  on  Robotics  and  Automation, 
vol.  6  no.  3,  June  1990,  pp.  382-388. 

Shoemake,  Ken,  "Animating  Rotation  with  Quaternion  Curves," 
Association  for  Computing  Machinery  _SIGGRAPH_  Proceedings, 
vol.  19  no.  3,  July  22-26  1985,  pp.  245-254. 

*/ 

n  1 1 1 1 1 1 / 1 1 1 11 1 1 11 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 11 1 1 1 1 li  I / n I  n  1 1 1 1 1 1 1 1 1 1 11 / 1 1 1 1 1 1 1 1 1 1 / 1 1 1  / 1 

«ifndef   QUATERNION_C 

#define  QUATERNION_C    //  prevent  errors  if  multiple  iincludes  present 

^include  "Vector3D.C" 

#define   PI     3.1415926535897932 

«ifdef  TRUE 
#undef  TRUE 
«endif 
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*:fdef  FALSE 
#undef  FALSE 
#fendif 

♦define   TRUE   1 
♦define   FALSE  0 

1 1 1  i I !:  i  1 1 1 1 1 1 !  1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 

! I    Utility  function  prototypes 


double  sign 

double  degrees 

double  radian? 

douole  arcclamp 


(double  X) 
(double  X) 
(double  X; 
(double  X) 


//  radians  input 
//  degrees  input 


double   dnormalize  (double  angle_radians) ;   //  returns  0..2PI 

int      inormalize  (double  angle_radians )    //  returns  0..359 

{return  (int)  (degrees  (angle_radians )  +  0.5)  %  360;} 

1 1  I !  ii  1 1 1 1 1 1 1 11 1 1 1 1  / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1  / 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 1  / 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 

class  Quaternion 


private: 

/,  meniDer 

data 

fields 

double 

qO; 

double 

q-; 

double 

q2; 

dcuDle 

q3; 

pUD^ IC : 

f.    constructors  and  destructor 
Quaternion 


Quaternion 
Quaternion 

Quaternion 

-Quaternion ( ) 

//  operators 

Quaternions,  operator  = 

Quaternion  operator  + 

Quaternion  operator  - 

Quaternion  operator  * 

Quaternion  operator  * 

doubled  operator  [; 


()  ; 

(const  double  phi, 

const  double  theta, 

const  double  psi ) ; 

(const  double  new_qO, 

const  double  new_ql , 

const  double  new_q2 , 

const  double  new_q3) 

(const  Quaternions  q) 

{  /*  null  body  */  ) 


(const  Quaternioni  q_rhs) 

(const  QuaternionSc  q_rhs) 

(const  Quaternions  q_rhs) 

(const  Quaternions  q_rhs) 
(double  scalar) ; 
(int) ; 


friend  ostreams  operator  <<  (ostreams  out,  Quaternions  q) ; 
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//    inspection  rriechods 


vcid 

double 
double 
double 

double 
Vector3D 


print 

ph^_value 

theta_value 

psi_value 

magnitude 
euler_angles 


Quaternion   conjugate 
Quaternion   inverse 

rr.odifying   methods 


void 


void 


rotate 


{); 

(  )  const 
( )  const 
I )  const 


//  Euler  angle  roll 
//  Euler  angle  pitch 
//  Euler  angle  yaw 


()  const;  //  Normally  1..0,  unit  quaternions 

{)  const;  //  Euler  angle  phi,   theta,  psi 
//  (roll,  pitch,  yaw) 

//  more  efficient  than  individual  calls 

()  const;  //  negates  vector  part 

{)    const;  //  negates  vector  part  &  normalizes 


(const  double  delta_phi, 
const  double  delta_theta, 
const  double  delta_psi   ); 


incremental_rotate  (const  double  P,  const  double  Q, 

const  double  R,  const  double  delta_t); 

double  R, 
double  seconds); 


double  rotation) ; 


void 

upaatc 

',  dcuole 

P, 

double  Q, 

void 

set 

(douDle 

phi , 

double  th 

void 

ncrTTial  :ze 

'  . 

••  ,  •  ////  !■!!■:  I '' i  I ;  1 1 1 1 1 1 1  i  1 1  n  1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 

double   sign    (double  X' 

if  (X   >   0.0)    return      1 .0 

else   if    I'x   <   0.0)    return   -1.0 
else  return      1 . Q 


/, 


■// 


double  dr-grfcs  (double  x)     //  radians  input 
return  x  '  l&C  .0  /  PI; 


/■ 


■// 


double  radians  (double  x)    //  degrees  input 
i 

return  x  *  PI  /  180.0; 
) 


II- 


■II 


double  arcclamp  (double  x) 


If 


(x  >   1.0) 


X  =   1.0; 

//  cout  «    "    arcclamp  reduced  "  «  x  << 
) 

else  if  (X  -^  -1 .0) 


to  1.0"  «  endl , 
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X  =  -1.0; 
//       cout  << 


arcclamp  raised  "  <<  x  «  "  to  -1.0"  «  endl 


return  x; 


■// 


douiiie   dncrmalize    (double   angle_radians ) 

double  new_angle  =  angle_radians ; 

while  (new_angle  :--  2'PI)  new_angle  -=  2*PI 
while  (new_angle  <  0.0  )  new_angle  +=  2*PI 
return  new_angle; 


■// 


//  constructor  methods 


Quaternion::  Quaternion  () 

go  =  1.0; 

ql  =  0.0; 

q2  -  0  .  0  ; 

q3  =  0.0; 


//■ 


■// 


Quaternion::  Quaternion 


(const  double  phi, 
const  double  theta, 
const  double  psi)  ■ 


//  reference:   Cooke  thesis  p.  41 

double  r,  p,  y,  cosr,  cosp,  cosy,  sinr,  sinp,  siny; 


r    =  (phi  /  2.0) ; 
cosr  =  cos  ( r) ; 
s.nr  =  sin  ir) ; 
:]i  =    icosr  '  cosp  * 
ql  =    'sinr  *  cosp  * 
ql    =         (cosr  *  smp  * 
qi  =  -  (sinr  *  sinp  * 


p 

= 

(theta  /  2.0); 

y 

=  (psi  /  2.0) 

cosp 

= 

cos  (p> ; 

cosy  : 

=  cos  (y )  ; 

sinp 

= 

sin  (p) ; 

smy  : 

=  sin  (y) ; 

cosy  , 

+ 

(Sinr  *  sinp  * 

siny)  ; 

cosy ) 

- 

icosr  *  sinp  * 

siny) ; 

cosy  ) 

+ 

(sinr  *  cosp  * 

siny)  ; 

cosy ) 

+ 

(cosr  *  cosp  * 

siny)  ; 

I 

// 

Quaternion::  Quaternion 


(const  double  new_qO, 
const  double  new_ql, 
const  double  new_q2 , 
const  double  new_q3 ) 


qO 

q2 

q3 


new_qO 
new_ql 
new_q2 
new_q3 


} 
//- 


•// 


•// 


Quaternion::  Quaternion  (const  Quaternions  q) 


qO  =  q.qO 
ql  =  q.ql 
q2  =  q.q2 
q3  -  q.q3 
) 

// 

/,'  operators 


■// 
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■// 


Quaternions  Quaternion::  operator  =  (const  Quaternions  q_rhs) 

qO  =  q_rh?.qfi 
ql  =  q_rhs.ql 
q2  =  q_rhs.q2 
q3  =  q_rhs.q3 
refuirn    *this; 


II- 


■II 


Quaternion  Quaternion::  operator  +  (const  Quaternions  q_rhs) 

i 

static  Quaternion  sum; 


sun-. .q';  =    qO  -    q_rhs.qO 

suiT^.ql  =   ql  +    q_rhs.ql 

sum.ql  =    q2  +    q_rhs.q2 

sum.  3?  =    q3  -r    q_rhs.q3 

return  sum; 


■// 


.'-tern:cr.  Q-aternion  :  :  operator 

static  Quaternion  difference; 

di f f erence .qO  =  qO  -  q_rhs.qO 

d^ f f trence .ql  =  ql  -  q_rhs.ql 

d: f f erence.q2  =  q2  -  q_rhs.q2 

di  f  f  trrence  .q3  =  q3  -  q_rhs.q3 
r^t;.-:-.  difference; 


(const  Quaternions  q_rhs; 


■// 


Quaternion  Quaternion::  operator  *  (const  Quaternions  q_rhs; 
{ 

static  Quaternion  prod; 


prod.qO  =  (qO  '  q_rhs.qO) 

- 

(ql  *  q_rhs.ql i 

(q2  *  q_rhs.q2) 

- 

(q3  *  q_rhs.q3) 

prod.ql  =  (ql  '  q_rhs.qO) 

+ 

(qO  »  q_rhs.ql) 

(q3  »  q_rhs.q2) 

-♦■ 

(q2  *  q_rhs.q3) 

prod.ql  =  (q2  ■*  q_rhs.qO) 

+ 

(q3  *  q_rhs.qli 

(qO  *  q_rhs.q2i 

+ 

(ql  '  q_rhs.q3) 

prod.:33  =  (q3  '  q_rhs.qO) 

+ 

(q2  *  q_rhs.ql) 

'ql  *  q_rhs.q2) 

+ 

(qO  *  q_rhs.q3) 

retur:,  prod; 

•// 


Quaternion  Quaternion::  operator 
( 

static  Quaternion  product; 

product. qO  =  qO  *  scalar; 

product. ql  =  ql  *  scalar; 

product. q2  =  q2  *  scalar; 

product. q3  =  q3  *  scalar; 
return  product; 


(double  scalar) 


II- 


■II 


doubles  Quaternion::  operator 
t 

if  (n  ==  0) 

( 


(int  n) 
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return  qO ; 

if  in  ==  1) 
{ 

return  ql ; 
) 
if  'n  ==  2) 

return  q2 ; 
} 

if  (n  ==  3) 
{ 

return  q3 ; 
} 
cout  <<    "Warning:  quaternion ["  <<  n  «  "]  is  an  invalid  accessor" 

<<•  "  (only  0..3  allowed),  returning  value  of  0.0"  <<  endl; 
static  double  dumiiny_value  =  0.0; 
return  duminy_value; 
1 
// // 

ostreaaii-  operator  <<    (ostreamSr  out,  Quaternions  q) 
( 

out  -'•'  "1"  <<   c.qCi  <<  ",  "  <<   q.ql  <<  ",  " 
--'<  q.q2  <<  ",  "  <<  q.q3  <<  "]"  ; 

return  (out ) ; 

) 

// // 

/•■  /  inspection  methods 

/  .' // 

void  juaternion::  print  () 
[ 

cout  <<  " i "  <<  qO  «  " ,     "  <<  ql  <<  " ,  " 

<<  q2  <<  " ,  "  <<  q3  <<  " ] "  ; 

} 

// // 

double  Guaternion::  phi_value  ()     //  Euier  angle  roll 

const 

return   acos  (arcclamp  ( (qO*qO  -  ql*ql  -  q2*q2  +  q3*q3) 

/  cos  {theta_value  ()))) 

*  sign   (q2'q3  ^  qO*ql } ; 

// // 

double  Quaternion::  theta_value  ()   //  Euler  angle  pitch 

const 
{ 

return  asin  (arcclamp  (-2.0  *  (ql*q3  -  q0*q2))); 

) 

// // 

double  Quaternion::  psi_value  ()     //  Euler  angle  yaw 

const 
i 

return   acos  (arcclamp  ( (qO*qO  +  ql*ql  -  q2*q2  -  q3*q3) 

/  cos  (theta_value  ()))) 

*  sign   (ql*q2  +  q0*q3) ; 

) 

// // 
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double  Quacerni on : :  magnitude 
const 

return  sqrt ( (qO  *  qO )  +  iql 

) 

// 


'  ql  )  +  (q2  *  q2)  +  (q3  *  q3 )  ) 


■// 


Vector3D  Quaternion::  euler_angles  ()     //  Euler  angle   phi,   cheta,  psi 

//  (roll,  pitch,  yaw) 

//  more  efficient  than  individual  calls 

const 

( 

double  qqO ,  qql,  qq2 ,  qq3,  phi,  theta,  psi,  costheta;  //  locals  hide  methods 

//  force  optimization  of  squaring  computations 


qqO 

qql 

:i'"i2 
qq3 


=  qO  ♦  qO 

=  ql  '  ql 

=  q2  •  q: 

=  q3  *  q3 


thetB    =  asin  farcclamp  (-2.0  *  (ql''q3  -  qO*q2))); 

costheta  =  cos  'theta!; 

phi      ~  acos  iarcclamp  (  iqqC  -  qql  -  qq2  +  qq3 )  /  costheta; 

'  sign   (q2*q3  ^  qO'ql ' ; 
psi      =    acos  (arcclamp  i  iqqC  +  qql  -  qq2  -  qq3 )  /  costheta) 

*  sign   (ql''q2  +  q0*q3'; 
return  Vectcr3E!  (phi,  theta,  psi )  ; 

)uacernior  Quaternion::  conjugate  ()    //  negates  vector  part 


const 
{ 

return  Quaternion  (qO,  -  ql,  -  q2 


q3); 


/, 


■// 


■// 


Quaternion  Quaternion::  inverse  ()    //  negates  vector  part 


:cn 


static  Quaternion  qresult; 

qresult  =  conjugate  ( )  ; 
qresult  .nonrialize  { )  ; 
return  qresult; 


■// 

■// 


//  modifying  methods 
// 


void  Quaternion::  rotate  (const  double  delta_phi, 

const  double  delta_theta, 

const  double  delta_psi   ) 
\ 

Quaternion  rotation; 

rotation. qO=  -0.5* ((ql  *  delta_phi)  + (q2  *  delta_theta) + (q3  *  delta_psi)); 

rotation. ql=   0.5* ((qO  •  delta_phi)  + (q2  *  delta_psi)   - (q3  *  delta_theta) ) ; 
rotation. q2=   0.5* ((qO  *  delta_theta ) + (q3  *  delta_phi)   -(ql  »  delta_psi)); 
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rotation. q3=   0.5* ((qO  *  delta_psi ;   + (ql  *  delta_theta j - (q2  *  del ta_phi ) ) ; 


qO  *-  rotation. qO 
q'i  -r=  rotation.ql 
q2  *=  iotation.q2 
q3  *=  rotation. q3 
no  rill  ji  ize  ;  /  ; 
return; 


7/ 


void  Quaternion 


increiTiental_rotate  (const  double  P,  const  double  Q, 

const  double  R,  const  double  delta_t; 


Quaternion  rotation; 


rotation 

.qO 

= 

-0 

5 

*   delta    t 

*     (  (ql 

« 

P) 

+     (q2 

rotation 

.qi 

= 

0 

5 

*    delta_t 

*     {  (qO 

* 

P) 

+     (q2 

rotation 

.q2 

= 

0 

5 

*    delta_t 

*     (  (qO 

* 

Q) 

+     (q3 

rotation 

.q3 

= 

0 

5 

*    delta_t 

*     (  (qO 

* 

R) 

+     (ql 

Q)  +  (q3  *  R) ) 

R)  -  (q3  *  Q) ) 

P)  -  (ql  *  R) J 

Q)  -  (q2  *  P)  ) 


qfi  +=  rotation.qCi 
qi  +=  rotation. ql 
q2  +=  rotation. q2 
q3  -t-  rotation. q3 
normal I ze  ( ) ; 
return; 


//■ 


■// 


void  Quaternion::  update  (double  F,  double  Q,  double  R,  double  seconds) 
( 

double     hh  =  seconds  *  0.5; 

doucie      h6  =  seconds  /  6.0; 

Quaternion  y      =    *tnis,    dynn,    dyt,    yt,    dydx; 


dyd>; .  qCi    = 

-0.5 

*     ( (ql    *    P)     - 

^     (q2    * 

Q) 

+ 

(q3 

*    R)  ) 

dydx.ql    = 

0.5 

'     ; (qO    *    P)    - 

^     (q2    * 

R) 

- 

(q3 

*    Q)  ) 

dyd>;.q2    = 

0.5 

'     uqO    *    Q)     - 

^     (q3    * 

P) 

- 

(ql 

*    R)  ) 

dy  d>: .  q  2    = 

0.5 

*     ( (qO    *    R)    ■ 

-■     (ql    * 

Q) 

- 

(q2 

*    P)  ) 

yt .qu    =   y 

.qu    * 

hh    '    dydy.  .qO 

yt.ql    -   y 

.ql    ^ 

h  n    *    dy  dx  .  q  1 

yt .q2    =   y 

.  q2    + 

hh    '    dyd>c.q2 

yt .q3    =    y 

.q3    - 

h  h    '    dy  dx  .  q  3 

dy  t  .  qCi    = 

-0.5 

*     (  (yt.ql    '    P 

+    (yt 

.q2 

* 

Q)    ^ 

(yt.q3 

*   R)  ) 

dyt.ql    = 

0.5 

'    ( (yt .qO    *    P 

+    (yt 

.q2 

* 

R)    - 

(yt.q3 

*    Q)  ) 

dyt.q2    = 

0.5 

"     ( (yt .qO    *    Q 

+    (yt 

.q3 

* 

P)    - 

(yt.ql 

*    R)  ) 

dyt.q3    = 

0.5 

►     (  (yt.qO    *    R 

+    (yt 

■ql 

* 

Q)    - 

(yt.c 

32 

*    P)  ) 

yt.qO    =  y . qO  +  hh  *  dyt.qO 

yt.ql    =  y.ql  +  hh  *  dyt.ql 

yt.q2    =  y.q2  +  hh  *  dyt.q2 

yt.q3    =  y.q3  +  hh  *  dyt.q3 


dym.qO    =    ■ 

-0.5    *     (  (yt .ql    *    P) 

+    (yt 

.q2 

dym.ql    = 

0.5    *     (  (yt .qO    *    P) 

+    (yt 

.q2 

dyin.q2    = 

0.5    *     (  (yt.qO    *    Q) 

+    (yt 

•q3 

dyni.q3    = 

0.5    *     (  (yt.qO    *    R) 

+    (yt 

•ql 

yt.qO    =   y 

. qO    +    seconds    *   dym 

qO; 

yt.ql    =  y 

. ql    +   seconds    *   dym 

ql; 

yt.q2    =  y 

. q2    +   seconds    •   dym 

q2; 

yt.q3    =   y 

. q3    +   seconds    •   dym 

q3; 

Q)  +  (yt.q3  *    R) ) 

R)  -  (yt.q3  *    Q) ) 

P)  -  (yt.ql  *    R) ) 

Q)  -  (yt.q2  *    P) ) 


dym.qO   =   dym.qO    +   dyt.qO; 
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dym.ql  =  dyir.  .ql  +  dyt.ql 
dym.ql  =  dym.ql  +  dyc.q2 
dyni.q3    =    dyrri.qS    ■>-    -dyt.qS 


( (yt .ql 
( (yc .qO 
( (yt .qO 
( (yt .qO 


dy-.:j;-- 

= 

-0 

c 

d\-t:  .ql 

= 

0 

5 

dyr .q2 

= 

0 

5 

dye .q3 

= 

0 

5 

qO  =  y 

.qO 

+ 

h6 

ql  =  y 

.ql 

+ 

h6 

q2  =  y 

•q2 

+ 

he 

qj  =  y 

q3 

+ 

h6 

nomrial. 

ize 

( 

; 

P; 

+ 

(yt 

.q: 

* 

C 

P) 

+ 

(yt 

.q2 

It 

R 

Q) 

+ 

(yt 

•q3 

* 

P 

R) 

4 

(yt 

■ql 

* 

Q 

+  (yt .q3  *    R) ) 

-  (yt.q3  *    Q)  ) 

-  (yt.ql  '    R)  ) 

-  (yt.q2  *    P) ) 


(dydx.qO  +  dyc.qO 

(dydx.ql  +  dyt.ql 

(dydx  .q2  +  dyt .  q2 

(dydx.qS  +  dyt.q3 


2.0  *  dym . qO ) 

2.0  *  dym.ql) 

2.0*  dym . q2 ) 

2.0*  dym.qB) 


(double  phi,  double  theta,  double  psi,  double  twist 


■// 


vci:i   ' 

;}uaterr.ion  :  : 

set 

(oouble  E 

jhi ,  dou 

qf 

= 

cos  (0.5 

*  twist 

ql 

=  cos  (Dhi ) 

* 

sin  (C  .  5 

*  twist 

q2 

=  cos  icneca 

sin  (0.5 

*  twist 

q3 

=  cos  (psi ) 

* 

sin  (0.5 

*  twist 

•// 


void  Quaternion 


norma  11 ze 


double  m,  =  maqnitude 
if  ( m  >  0  .  0  ) 


a 

/  =  rri  ; 

ql 

q2 

/  =  mi  ; 
'  =  m  ; 

i 

q-3 

/=  m; 

1 

// 


1 1  n  1 1 1 1 11 1 1 11 1 1 / 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

#endif         //    »tifndef    QUATERNION_C 


257 


K. 


Vectors D.C  3D  Vector  Mathematics 


!!!  1  I  !!.■  i  :/!  i  I  1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  !  1 1 1 1 1 1 1 1 1  I  i  1 1 1 1 1 1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 
ProqiaiTi:  Vector3D.C 

Original    Author:    Keith   Haynes 


Revi?ion? : 
h^v:  .5ed : 
Systerr;: 
Comp  i 1 e  r : 
Comp;:  2  at  ion 

Description 
Advisors : 


Don  Erutzman 

12  FEE  94 

Ins /PC 

ANSI  C++ 

irix--  CC  VectorBD.C  -Im  -c 


-C   =: 


Produce  binaries  only,  suppressing  the  link  phase 
+w  ==  Warn  about  all  questionable  constructs. 

Vector3D  class  specifications  and  implementation 

Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 


1 1  :'!■ 1 1 !  i  I !!! 1 1 1 n  1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

II   prevent  errors  in  multiple  ^includes  present 


#ifndef  VECT0R3D_C 
*def.ne  VECT0R3D_C 


# include  ^-iostrearr. .  h> 
«in~iude   --icrriani 


<-  h  . 


♦t^riclude  --math-h;- 

cias?  VectorSE' 

( 

/ /  memDer  data  fields 


aou.oie  X 
dcuii-Itr  y 
double    2 


put; 


//  member  constructor  and  destructor  functions 


Vector 3D 
Vector3D 
VectorBD 
-Vector3D  0 


//  operators 


(double  a,  double  b,  double  c) ; 
(const  Vector3D&); 
i  /*  null  body  */  ) 


(const  Vector3D&) 


VectorBD  k   operator  . 

VectorBD  operator  +  (const  Vector3D& 

Vector3D  operator  -  (const  Vector3D&), 

double  operator  *   (const  Vector3D&);   //  dot  product 

VectorBD  operator  *  (double) ;  //  scalar  multiplication 

VectorBD  operator  /  (double) ;  //  scalar  division 

VectorBD  operator  "  (const  Vector3D&);   //  cross  product 

double  &  operator  [)  (int)  const; 


//  inspection  methods 
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double     magnitude    (); 

friend     oscream  k   operator  <<  (ostreain&  os,  VectorBDSt  v) 

void       print  ( ) ; 

iTiodi  f-.-ip.o  methods 


void 
void 


normalize        ( ) ; 
noriTialize        (double) 


I !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

//default  constructor 
Vector3D: :Vector3D( ) 


X  =  0  .C 
y  =  0.0 

2^0.0 


//constructor  using  three  doubles 

Vector3D: :Vector3D (double  a,  double  b,  double  cl 


a  ; 
b; 


■  const rjctor  using  another  Vector3D 
Vector3D:  :Vector3D(const  Vector3D&:  vi 

X  =  V  .  X ; 
y  =  v.y; 
Z  =  V  .  z  ; 


//Assigmiient  operator  -  the  function  must  return  a  reference  to  a  Vector 
//instead  of  a  Vector  for  assignment  to  work  properly 
Veccor3Di<  Vector3D:  : operator=  (const  Vector3D6<  v) 


X  =  V  .  X  , 
y  =  v.y, 
z  =  V. 

return  *this; 


//Vector  addition  operator 

VectorJL  Vector3D: : operator+ ( const  Vector3D&  v) 

Vector3D  sum; 
sumi.x  =  v.x  *   X; 
sum.y  =  v.y  +  y; 
sum  .  z  =  V  .  z  -1-  z  ; 
return  sum; 
) 

//Vector  substraction 

Vector3E;  Vector3D  ::  operator- (const  Vector3DSc  v) 

( 

Vector3D  diff; 

diff.x  =  X  -  V.X; 

diff.y  =  y  -  v.y; 

diff.z  =  z  -  v.z; 

return  diff; 


//Vector  dot  product 


259 


double  VeccorJD :: operator* ( const  Vector3D&  v) 
{ 

double  dot; 

doc  =  'v.x  '  xi  +  (v.y  *  y;  -►  (v.z  '  z); 
return  dot; 

//scalar  multiplication 

Vector3D  Vector3D :: operator* (double  n) 

{ 

Vector 3D  mul t ; 
mui  t .  X  =  X  *  n; 
mu 1 t . y  =  y  *  n ; 
mu 1 1  .  z  =  z  *  n ; 
return  mult; 
} 

//scalar  division  -  it  is  the  user  responsibility  to  make  sure  that  n  is  not 

zero 

Vectorjl;  Vector3D  ::  operator  /  (double  n) 

( 

Vector 3D  result ; 

result .X  =   X   /  n; 

result .y  =  y  /  n; 

result . z  =  z  /  n; 

return  result; 
} 

//Vector  cross  product 

Vector3D  Vector3D  ::  operator'^  (const  Vector3D&  v) 
i 

Vector3D  cross; 

cross. X  =    iy*v.z;  -  (v.y*z); 

cross.y  =  - i (x  *  v.z)  -  (v.x  '  z)); 

cross. z  =    (X*v.y)  -  {v.x*y); 

return  cross; 
} 

//the  <<  operator  is  to  be  used  witn  output  stream  out 
ostream.&'  operator  <<    (ostreami  out,  Vector3DSf  v) 

c.  t  -•  "["    ■■:   v.x  -^  <    ",  "  -- •'  v.y  <<  ",  "  <<  v.z  <<  "]"; 

return  (out i ; 

void  Vector3D::  print  () 
( 

cout  <•<  "  I"  «  X  <<  ",  "  <<   y  <<■  ",  "  <<  z  <<  "]  "; 

//allows  access  to  the  components  of  the  Vector3D.   it  must  return  a  reference 

//in  order  for  assignment  to  work 

doubled  Vector3D: :operator [ ] (int  n) 

const 

{ 

static  double  result; 

if  (n  ==  1) 

{ 
result  =  x; 

return  result; 

) 

if  (n  ==  2) 

{ 
result  -   y; 

return  result; 

} 
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if  (n  -=  3) 
t 
result  =  z; 

return  result; 
\ 

cout  •---  "Warning:  Vector3Di"  <<  n  <<  "  ]  is  an  invalid  accessor" 
<<  "  (only  1..3  allowed),  returning  value  of  0.0"  «  endl; 

static   double   dumiTTy_value   =    0.0; 
return  dummy _value; 
} 

//returns  the  magnitude  of  the  Vector 

double  Vector3D: :magnitude ( i 

( 

return  sqrt ( ix  *  x;  +  (y  *  y)  +  (z  *  z)); 
) 

•' 'normai  i  zes  the  Vector  to  one 
void  VectorBD ;: normal ize ( ) 
{ 

double   m    =    sqrt ( (x    *    x)    +     (y    *    y)    +     (z    *    z) ) ; 

if     (mi 


X 

'   m 

y 

'   m 

z 

'    m 

//normalize  the  Vector  to  d 

void  Ve-.:tcr3D:  :  normalize  (double  dj 

double  m  =  sqrt ' (x  *  x)  +  (y  *  y)  +  (z  •  z)); 

if  in., 

( 

X  =  d  *  X  /  m; 
y  =  d  *  y  /  m; 
z  =  d  *  z  /  m; 

» 

1 1 1 1 1 1 1 ;  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11  n  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1  i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

«endif    //  «ifndef  VECT0R3D  C 
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L.  Makefile  for  Hydrodynamics  Classes 


#  MaK.<7'f-i&  dynamics  model  for  underwater  virtual  world 
«  14  October  94    Don  Brutzman 

sg:irix4flags  =  -02 

«  DIS  includes 

INCf  =  - I/n/dude/work/brutzman/DI£ .mcast /h 

LI&_L'IR  =  -L/n/dude/work/brutzman/DIS  .mcast/src 

LIB  -  /n/dude/wcrk/brutzrtian/DIS  .nicast/src/libdis_client  .a 

LIE_AP,G  =  -ldis_client   -Impc  -Im 

HEip._?'ATK  =  /n/dude/work/brutzman/DIS  .mcast /h/ 

HlihZ  =  S  (HDR_PATHjpdu.h  $  (HDR_PATH )  disdef  s  .  h 

CCFLAGS  =  +w  $ (SGIIRIX4FLAGS) 

#INC£  =  -I/n/elsie/work3/macedoni /net/mcast/network/h 

#LIB_DIR  =  -L/n/elsie/work3/macedoni /net/mcast/network/bin 

#LIE  =  /n/elsie/work3/macedoni /net /mcast /network /bin /I ibdis_cl lent .a 

«LIE_ARG  =  -ldis_client  -Impc  -Im 

#HDR_PATH       =  /n/elsie/work3/macedoni/net/mcast/network/h/ 
#HDFS  =  S ;HDR_PATK;pdu.h  S (HDR_PATH) disdef s . h 

#•'£""_:  I-'  =  /n/elsie/work3  /macedoni/net /mcast/network/src/ 

#NET_MR  =  /n/dude/worK/brutzman/DIS. mcast/src 

#NETOBJS  =  S (NET_DIR; attach. o  $ (NET_DIR) client_lib. o  \ 

#  S ;NET_DIR) free.o    S (NET_DIR) mallocs . o  $ (NET_DIR) sends . o    \ 

#  S (NET_DIR) recvs.o   $ (NET_DIR) protocol . o 

OBJE  =  dynamics. c     AUVsocket.c    Vector3D.o  Quaternion.©  \ 

Hmatrix.c      RigidEody.o     DISNetworkedRigidBody .oUUVBody .0    \ 
SonarModel.o    math_util it ies  .  o 

dynamics:  S(LIB)  $(HDRS)  $(OBJS) 
(Sec.'-iG  "Linking ..." 

CC  $ (SGIiRIX4FLAGS)  S(LIB_DIR)  $(OBJS)  $(NETOBJS)  ${INCS)  ${LIE_ARG)  \ 
-o  dynamics 

«  Makefile  requirement  is  that  no  libraries  are  linked  to  the  objects 

Vector3D.o:  Vector3D.C 

CC  $(SGIIRIX4FLAGS)  $(LIB_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 
-c  Vector3D.C  -o  Vector3D.o 

Quaternion. o:  Quaternion. C  Vector3D.C 

CC  $(SGIIRIX4FLAGS)  $ (LIB_DIR)  $(NETOBJS)  $(INCS)  $ (LIB_ARG) \ 
-c  Quaternion. C  -o  Quaternion. 0 

Hmatrix.o:  Hmatrix.C  Vector3D.C  Quaternion. C 

CC  $(SGIIRIX4FLAGS)  $(LIB_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 
-c  Hmatrix.C  -o  Hmatrix.o 
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KioidBccry.o:    RigidEody.C   Hmacrix.C 

CC  $ (SGIIRIX4FLAGS)  $(LIB_DIR)  $(NETOBJS)  S(INCS)  S(LIB_ARG)\ 
-c  RigidEody.C  -o  RigidBody.o 

AWsocket.o:  AUVsocket.C  AUVglobals.H  UUVmodel.H  Quaternion. C  S  (HDRS) 
CC    S (SG:IRIX4FLAGS;  $(LIB_DIR)  $(NET0BJS)  $(INCS)  $(LIB_ARG)\ 
-c  Ain/socket.C  -o  AlTVsocket.o 

riSNetworkedRigidBody .0:  DISNecworkedRigidBody . C  AUVglobals.H  UUVmodel.HX 
RigidBody .C  $ (HDRS) 
CC  $(SGIIRIX4FLAGS)  $(LIB_DIR)  $(NET0BJS)  $(INCS)  $(LIB_ARG)\ 
-c  DISNecworkedRigidBody .C  -o  DISNecworkedRigidBody . o 

ScnarModel . c:  SonarModel.C 

CC  S (SGIIRIX4FLAGS)  $(LIB_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 
-c  SonarModel.C  -o  SonarModel.o 

UWBody.o:    UU\^Body.C  AlTVglobals  .H   Tjuvmodel  .  H   SonarModel.C  AUVsocket.C\ 
DISNetworkedRigidBody .C 
CC    $(SGIIRIX4FLAGS)     S(LIB_DIR)     S(NETOBJS)     $(INCS)     $(LIB_ARG)\ 
-c   UUVBody.C    -o  UUVBody . o 

dy.-.arrii  c?.c:    dynamics. C  AlTVglobals  .  H   UUVmodel.H  UUVBody.C   S(HDRS) 

CC  C jSGiIRIX4FLAGS)  $(LIE_DIR)  $(NETOBJS)  $(INCS)  $(LIE_ARG)\ 
-c  dynamics. C  -o  dynamics. o 


r^-.ic. •'.__-. .  I  ities  .  c  :  marh_ut  il  ities  .  C 

CC  $  (SGI:RIX4FL.AGS!  S(LIE_E:IR)  S(NETOBJS)  $(INCS)  S(LIB_ARG)\ 
-c  mat:h_ui:ilities  .  C  -o  math_ut  il ities  .  o 

*  $(OECC::  AUVglobals.H  UUVmodel.H  $(HDRS) 

*  CC  -c  r'CCFLAGS)  S ( INCE ;  S*.C 

*  utilities  : 

delete: 

rrr    -f    rore    '.o   '.a   a.  out    di.'namics 

clear;: 

rm.   -f   core   *.o   *.a   a.  out 

all :  delete  dynamacs 
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VI.   SONAR  MODEL 
A,  SonarModel.C  Geometric  Sonar  Model 

1 1  i  I  i  i  1 1 1 1 1 1 1 1 1  n  1 1 1 1 !  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1  / 1 1 1 1 1 

Pi'cciran.:  SoiiarModel  .C 

Description:  Underwater  vehicle  geometric  sonar  model 

Revised:  21  October  94 

Sy.-.en-, :  Irix 

Ccmpiier:  ANSI  C-f  + 

Ccmiiilaticn  :       irix>  make  dynamics 

irix>  CC  SonarModel.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 
+w  ==  Warn  about  all  questionable  constructs. 

Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Author:  Don  Brutzman  brutzman@cs.nps.navy.mil 

Code  OR /Br 

Naval  Postaraduate  School  (408)  656-2149  work 

Monterey  CA  93943-500C  (408)  656-2595  fax 

Re f  erer.ces : 

Status:  Only  pool  geometric  model  included 

Future  work:       Comirients  and  suggestions  are  welcome! 


I i 1 1 1 1 1 1 1 1 1 1 1 i 1 1 1 li I ! 1 1 1 1 1 1 1 1 1 1 1 n 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 

#ifndef  SONARMODEL_C 

#define  SONARMODEL_C    //  prevent  errors  if  multiple  #includes  present 

^include  "AUVsocket .C" 

int    precede         (double  anglel ,  double  angle2); 

int    precede_radians  (double  anglel,  double  angle2); 

void   test_tank_sonar_model  ( ) ;   //  parallelize  &  generalize  this  sonar  model 

//  for  all  objects  in  vw,  using  Ziomek's  RRA 
double  radian_normalize   (double  rads); 
double  radian_normalize2  (double  rads); 

1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 H 1 1 11 1 1 1 1 1 1 1 1  H  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 

I* ./ 

/*  excerpt  from  geometric  reasoning  circle  world  circle. c  */ 


264 


/*  Boolean  function  for  angle  precedence  */ 

inc    precede  (double  anglel,  double  angle2) 

/*  angles  are  any  real  angles  in  degrees  *l 
I*  return  TRUE  if  anglel  precedes  angle2,*/ 
/'    return  FALSE  otherwise  */ 

{ 

/'  note  that  the  input  angles  are  individually  normalized  to  ensure  validity   */ 

if  (normalize2  (normalize2  (angle2)  -  norinalize2  (anglel))  >  0.0) 

return  TRUE; 
else  /*  reference:   equation  (7)  class  notes   */ 

return  FALSE; 


double  radian_norinalize  (double  rads)     /*  radians  input*/ 

double  result  -  rads; 

while  .'result  -.-       0.0)  result  +=  2.0  *  PI; 
while  '.result  >=  2.0  *  PI)  result  --  2.0  *  PI; 

return  result; 


doubie  r5Giari_nonTiai  1  ze2  (double  radsi      /*  radians  input*/ 

dcuKl-r  result  =  rads; 

while  (result  <=  -  PI)  result  +=  2.0  *  PI; 
whilrr  'result  >    PI)  result  -=  2.0  *  PI; 

return  result; 

,> */ 

/'  excerpt  froir,  geometric  reasoning  circle  world   circle. c  */ 

/*  Boolean  function  for  angle  precedence  */ 

mt    precede_radians  (double  anglel,  double  angle2) 

/*  angles  are  any  real  angles  in  degrees  */ 
/■*  return  TRUE  if  anglel  precedes  angle2,*/ 
/*    return  FALSE  otherwise  */ 

/■*  note  that  the  input  angles  are  individually  normalized  to  ensure  validity   */ 

if  (radian_nonnalize2  (radian_nonnalize2  {angle2)  - 

radian_normalize2  (anglel))  >  0.0) 

return  TRUE; 
else  /*  reference:   equation  (7)  class  notes   */ 

return  FALSE; 

) 

/» */ 
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void  ce;?r_tar.k_son5r_model  {)    /  /  parallelize  &  generalize  this  sonar  model 
I 

df;^cie  SA,  £B,  SC,  SD;  //  angles  from  sonar  to  corners 

X-axis 


//  Test  tank  coordinates: 

// 

// 

// 


A  (-10,   10]   +--+   (10,   10)  B 

I   I 

I   I  I 

D  (-10,  -10)   +--+   (10,  -10)  C      +->Y-axis 


double  return_x,  return_y,  distance;   //  intersect  coordinates  &  distance 
//  AUV_£T1000_bearing  and  AUV_ST7  2  5_bearing  determined  by  vehicle  control 


Airv_iT1000_range   =  0.0; 

A'JA/_ST1000_st  renqth=  0.0; 


A'.T'.-_£T72  5_ranae    =  0.0; 
AUV_ST72  5_strength  =  0.0; 


//  ST_1000  conical  pencil  beam  bearing 

//  ST_1000  conical  pencil  beam  range 

//  ST_1000  conical  pencil  beam  strength 

//  ST_725  1  X  24  sector  beam  bearing 

//  ST_7  2  5  1  X  24  sector  beam  range 

//  ST_725  1  X  24  sector  beam  strength 


double  sonar_x        =  AUV_x   +   cos    (AU\'_psi)    *  AUV_ST1000_x_of  f  set ; 
double  sonar_>'        =  AUV,:^'    +    sin    {Air7_psi)    *  AUV_ST1000_x_of  f  set ; 

//    remove   these   lines  when   auv  vw  viewer   includes   sonar  offset 
sonar_x  =   AW_x; 

sGnar_y  =  AU\/'_j/ ; 

double   sonar_psi    =   Air.'_p>si    +    radians    (AUV_ST1000_bearing)  ; 

if     ;  tAW_x   <=    10.0)    &£,     (AU\'_x    >=    -10.0)    && 

;Ar'0'  <=  10.0 J   icS.    (Auv^  >=  -10.0;   i^h. 

(AUV_z    <=    50.0)     i&     iAUV_z    >=       41.0)) 


{ 


A'-".'_rT10C0_strength=  10.0, 
AU\'_£T7  25_strength  =  10.0, 


//  return  expected 
//  return  expected 


//  note  that  a  reversed  x,y  calling  sequence  is  necessary 

/  /    m  order  to  get  correct  quadrant  alignment 

SA  =  atan2  ((-IC.O;  -  sonar_v',  (  10.0)  -  sonar_x) 

Sb  =  atan2  ((  10.0)  -  sonar_y,  (  10.0)  -  sonar_x) 

SC  =  atan2  ((  10.0)  -  sonar_y,  (-10.0)  -  sonar_x) 

SL  =  atan2  (;-10.0)  -  sonar_y,  (-10.0)  -  sonar_x) 


"AUV_x  =  "    «  AUV_x 
AW_y  =  '■    <-.  AUV_y 

AUV_psi  =  "  <<  degrees  (AUV_psi)  <<  endl ; 
"sonar_x  =  "    «  sonar_x 
sonar_y  -    "        «   sonar_y 

sonar_psi  =  "  «  degrees  (sonar_psi)  <<  endl; 
"SA  =  "      «  degrees  (SA) 
SB  =  "      «  degrees  (SB) 
SC  =  "      «  degrees  (SC) 
SD  =  "      «  degrees  (SD) 
<<  endl ; 
cout  «     "precede_radians  (SA,  AUV_psi)  =  " 

«     precede_radians  (SA,  AUV_psi)  «  endl; 


if 

(T.RAC 

■-' 

i 

cout 

« 

<< 

" 

<< 

cout 

« 

<< 

"  , 

« 

" 

cout 

« 

« 

" 

« 

« 

" 

} 

if 


1    precede_radians  (SA,  AUV_psi) 
&&  precede_radians  (AUV_psi,  SB)) 
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{ 

if  (TRACE;   cout  <■'  "SECTOR  I"  «  endl  ; 

return_x  =  10.0; 

retjrn_y  =  sonar_y  +  sin  (sonar_psii  *  (10.0  -  sonar_x); 

el?e  if  (    precede_radians  (SB,  sonar_psi) 

&&  precede_radians  (sonar_psi,  SC) ) 
{ 

If  (TRACE)   cout  <<  "SECTOR  II"  <<  endl ; 

return_x  -    sonar_x  -  sin  (sonar_psi  -  PI/2.0)  *  (10.0  -  sonar_y); 

return^-  =  10.0; 
) 
else  if  (   precede_radians  (SC,  sonar_psi) 

kk   precede_radians  (sonar_psi,  SD) ) 
{ 

if  (TRACED   cout  «  "SECTOR  III"  <<  endl; 

reti:rn_x  =  -  10.0; 

recurn_y  =  sonar_y  -  sin  (sonar_psi  -PI)  *  (sonar_x  +  10.0); 
'» 

elL-e  if  !   precede_radians  (SD,  sonar_psi) 
kk   precede_radians  (sonar_psi,  SA) ) 

if  (TRACE)   cout  «  "SECTOR  IV"  «  endl; 

recurn_x  =  sonar_x  +  sin  (sonar_psi  +  PI/2.0)  *  (sonar_y  +  10.0); 

ret-jrn_:>-  =  -  10.0; 

el?e  cout  '••-  "Computational  sonar  error,  no  sector  was  determined." 

•r-.  endl  ; 

distance  =  sort  '   freturn_x  -  sonar_xl  '  (return_x  -  sonar_x! 

+  (return_>'  -  sonar_y)  *  (return_y  -  sonar_y)); 

Al^.'_STl-')nn_range  =  distance; 
Al^'  ST'725  ranqe  -   distance; 


If  (TRACE^ 

1 

cout  •'•' 

"return_x  = 

<< 

,  return_y  = 

«  return_x 

«  return_y  <<  endl ; 

cout  ■'<        "AUV_ST1000_range  -    "    «   AUV_ST100  0_range 

«  ",  AUV_ST725_range  =  "   «  AUV_ST7  25_range  «  endl; 

} 

else  return,  //  outside  of  test  tank,  default  return  is  0 

}   //  end  test_tan)':_sonar_model 

i :  I  i  I !  I .:  i  1 1 1 1 i  I  i  1 1 1 i  I !  I i  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ! 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 1 1 1 1 1 1 1 1 1 1 11 1 1 1 

«>endif  ■ ,  SONARMODEL  C 
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VII.   NETWORKING 

A.  Introduction 

The  principal  networking  code  used  in  the  underwater  virtual  world  has 
already  been  presented  in  previous  listings.    Source  programs  execution.c  and 
AUVsocket.c  implement  socket  communications  between  robot  and  virtual  world,  while 
DlSNetworkedRigidBody.C  and  viewer. C  communicate  via  DIS  PDUs  over  the  MBone. 
These  communications  links  were  not  created  from  scratch.    The  following  test 
programs  were  used  in  program  development  and  are  included  as  valid  standalone 
applications  that  can  be  used  to  test  network  software,  independently  of  the  robot  or 
computer  graphics  applications. 

os9sender.c  and  os9server.c  are  a  client/server  pair  used  to  open  test  and  close 
a  point-to-point  socket   What  is  unusual  about  these  programs  is  that  they  were 
developed  to  operate  interchangeably  on  either  Unix  systems  using  Berkeley  Software 
Distribution  (BSD)  4.3  network  routines,  or  on  OS-9  processors  using 
Microware -provided  network  routines  (Microware  91a,  91b).    Network  code  is  difficult 
to  debug  so  extensive  trace  options  are  provided  to  assist  in  diagnosing  difficulties. 

Prior  to  the  current  multicast  version  (2.0.3)  of  the  DIS  library  distribution,  all 
DIS  interfaces  used  broadcast  or  unicast  transport  protocols.   This  meant  that  all 
network  interactions  between  DIS  applications  were  restricted  to  a  single  local  area 
network  (LAN).    Such  a  limitation  is  unacceptable  if  an  underwater  virtual  world  is  to 
be  capable  of  communicating  globally  across  the  Internet,   disbridge.c  was  written  to 
create  a  socket  bridge  between  LANs  which  reflected  DIS  PDUs  identically  in  either 
direction.   Multiple  disbridge  programs  could  connect  arbitrary  numbers  of  LANs  with 
only  a  single  disbridge  needed  for  each  new  network  addition.   Although  such  an 
approach  is  inconvenient  and  a  somewhat  inefficient  use  of  bandwidth,  it  can  extend 
the  broadcast/unicast  DIS  approach  to  large  numbers  of  networks. 

disbridge  has  been  tested  successfully  with  earlier  versions  of  the  underwater 
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virtual  world,  and  also  with  the  NPS  Platform  Foundation  (Bailey  94).   Fortunately 
disbridge  is  no  longer  needed  since  the  newer  multicast  DIS  version  2.0.3  can  take 
advantage  of  the  MBone  to  attain  Internet-wide  connectivity  (Zeswitz  93). 
Nevertheless  only  about  a  thousand  subnets  are  currently  connected  to  the  MBone,  and 
conceivably  disbridge  might  someday  be  useful  to  connect  networks  independently  of 
the  MBone.   Recommended  future  work  includes  upgrading  disbridge  to  be  compatible 
with  future  versions  of  the  DIS  library  in  unicast,  broadcast  or  multicast  modes. 


269 


B.  os9sender.c  Unix  to  OS-9  Socket  Communications  Client 

Program:  os9sender.c 

Ijescripcion:       socket  test  program  to  pass  input  strings  in  2  directions, 

especially  socket  from  OS-9  auvsiml  to  Irix  fletch.cs 

Revised:  31  MAY  94 

Compilation:       unix>  cc  os9sender.c  -o  os9sender 
os-9>  make  os9sender 

Execution:         nostl~>   os9server 

host2--   os9sender  -r  hostl 

Example:  fletch>   os9server 

auvsiml>  os9sender  -r  fletch 

Execution  options: 

-r  -h  -t   'first  letters  are  sufficient,  capital  letters  are  OK  too) 

-remote  hostnamie   hostname .  remote  .net  .address  for  client  to  connect  to  server 
no  default  is  present 

-help  display  calling  syntax 

-trace   turn  on  TRACE  mode 

References:     (1)  (Gespac-provided)  EVIRA  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2;  Internetworking  with  TCP,'IP  Volume  I:   Principles, 
Protocols  and  Architectures,  Douglas  E.  Comer, 
Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:   Design, 

Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(4!  IRIX  Network  Programming  Guide,  Silicon  Graphics  Inc. 

(5;  An  Advanced  4.3BED  Interprocess  Commiuni cation  Tutorial, 
Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 
Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 

(6)  Real-Time  Programirang  Tutorial,  Bill  Mannel ,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 
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Aucnor 


b  t  a  r  u  s  : 


Don  Bruczman 

Code  OR/Br 

Naval  Postgraduate  School 

Monterey  CA  93943-5000 


brutzmanScs . nps . navy .mil 

(408)  656-2149  work 
(408)  656-2595  fax 


Tested  satisfactory  Irix<==>Irix,  0S-9<==>0S- 9 ,  Irix<==>0S-9 


Future  work:       Consider  implementation  as  an  inetd  ' superserver'  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available. 

add  #elif  (sun)  defines 

Comments,  suggestions  and  corrections  are  welcome! 


:  t  ■****•**  •: 


«^ : 


:r:x  aefine? 

defined • sgi 


in  /usr,' include  '/ 


^include  <'stdic.h> 
«include  <sys. ■' types  .h> 
^include  <sys/ socket . h> 
♦include  <netinet/in . h^ 

♦  include  -netdb.h-- 
♦include  <signal.h> 
♦include  <string.h> 

'*  0£-:-  .-uLdi  rectories  ''uEFi 

♦  c-^se 

♦include  -srdic.h^ 

♦  include  --types. h> 
♦include  ^inet /socket .h> 
"include  --i  net  /  in  .  h-- 

♦  include  -^inet /netdb.  h> 
» include  <£ignal.h> 
♦include  <errno.h> 

♦endif 

♦include  <time.h> 


/DEFS/INE1 


/*  One  jtreaiTi  socket  is  used  with  adequate  throughput 

/'    !alc.*"iOugh  two  could  work,  no  performance  improvement  is  expected) 

/*  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
/*     from  other  processes  requesting  ports  on  your  system: 


♦define  DISBRIDGE  TCP  PORT 


♦define 
♦define 
♦define 
♦define 
♦define 
♦define 
♦define 
♦define 


AUVSIMl. 
AUVSIMl. 
AUVSIMl. 
AUVSIMl. 
AUVSIMl. 
AUVSIMl. 
AUVSIMl. 
AUVSIMl 


TCP. 
TCP 
TCP. 
TCP 
TCP 
TCP 
TCP. 
TCP 


PORT_0 
P0RT_1 

.P0RT_2 
P0RT_3 

.P0RT_4 
P0RT_5 
P0RT_6 
PORT  7 


2056  /*  disbridge  program,  server  &  client     */ 

/*  NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 

/•  Underwater  Virtual  World  (UVW)     */ 

3210  /*  os9sender  <==>  os9server  test  programs  */ 

3211  f  auv  execution  level  <==>  virtual  world  */ 

3212  /*  port  for  future  use           */ 

3213  /•  port  for  future  use           */ 

3214  /*  port  for  future  use           */ 

3215  /*  port  for  future  use           */ 

3216  /*  port  for  future  use           */ 

3217  /*  port  for  future  use           */ 


271 


«define  AI'\'£:m1_TCP_P0RT_8 
#define  AUV£IM1_TCP_P0RT_9 

*  define  TRUE   1 

#  define  FALSE  0 


3218  /* 

3219  /♦ 


port  for  future  use 
port  for  future  use 


«define  £OCKET_QUEUE_SIZE 


/'  max  allowed  by  TCP/IP 


/'  function  prototypes  *' 


•  *  *  *  *  -t  *   *  : 


■*********i 


*****♦*««*■ 


■*****: 


/  ■•  Note  that  function  prototype  parameters  may  need  to  be  deleted  if  the 
C  compiler  used  is  not  an  ANSI  compiler  (e.g.  OS-9  K£cR  C  compiler) 

void  shutdown_o£-9?ender  (  )  ; 


global  variable  definitions  ****** 


static  int 

static  int 

static  int 

static  int 

int 

static  int 


/*  1  =  trace  on,  0  =  trace  off  */ 


TRACE  =  0; 

socket_descriptor, 

socket_accepted, 

socket_strearT,; 

socket_length  =  25  5; 


by tes_received,  bytes_read,  bytes_written, 
bytes_left,  bytes_sent; 

shutdown_signal_received  =  FALSE; 

i  =0, 

print_help     =  FALSE; 


:«**«*• 


:******«**«**«**«*****/ 
:*«*******»«*»«******»/ 


main  largc,  argv) 

int  argc;  char  '*arav; 
{ 

char 

FILE 

struct  sockaddr  in 


/*  command  line  arguments  */ 

command_sent  [81], 

command_received    [81], 
remote_ho3t_naiTie    [60]; 

•netstat_f ileptr ; 

server_address ; 


register  struct  hostent  *server_enti ty ; 
char  test_buffer  [81]; 


static  char 
static  char 


*ptr ; 
*remote_buf f er ; 


/*  Begin  execution,  parse  commiand  line  */ 

for  (1  =  1;  i  <  argc;  i++) 
( 

if  ((argv[il[0]  !=  '-')  II  print_help) 

{ 
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print_help  =  TRUE; 
break; 


swjtcn  ;ara'. 


;  ill;    /'  swirch  based  on  current  command  line  parameter  */ 


case  'h' :  /'  help  */ 
case  'K' :  /*  Help  */ 
case  ' ?' :       /*  Help  */ 

printjielp  =  TRUE; 

break ; 


case  ' r '  :  /•  remote  hostname  tells  client  who  to  connect  to 

case  ' R ' : 

/*  get  remotehost  name  */ 

if  (i+l  <  argc)  strcpy  (remote_host_name,  argv[++i]); 

else  print_help  =  TRUE; 

break ; 


case  ' t '  : 
case  'T' : 

TRACE  =  1 ; 

break ; 


/■*  TRACE  feature 


default:       /  ■•  nc  command  line  entry  parameter  present     */ 
print_nelp  =  TRUE; 

>  /*  end  switch  based  on  current  command  line  parameter  */ 

;■     /  '  end  for     '  / 

if  (argc  ==  l  i  l  print_help)  /'  print  help  string  **«««*••♦•'»«»««*»♦»«*«"■ 

prir.tf  :  "Usage:   os93ender  -remote  hostname  [-helpj  ( -trace]  \n"  )  ; 
f XI t  i -  1  ;  ; 


if  (TRACED  printf ' " ios9sender  TRACE  on]\n"); 
f  f  ^us;.  !  stdin;  ; 

/'  Comrr.and  line  parameter  parse  complete 


/  •*  Initialize  communications  blocks 


r  *»«*«*  1 


:**«***«*«****** 


/■*  set  socket_length 
socket_length  =  81; 


/*  maximum  allowed  packet  size 


/*  Initialize  both  client  ^    server 


;***«*******• 


/'  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()'/ 
/*    signal  handlers.   Othervi'ise  you  are  unable  to  "C  kill  this  program.   */ 


Mil   defined (sgi ) 

signal  (SIGHUP,  shutdown_os9sender ) 

signal  (SIGINT,  shutdown_os9sender ; 

signal  (SIGKILL,  shutdown_os9sender ) 

signal  (SIGPIPE,  shutdown_os9sender ) 

signal  (SIGTERM,  shutdown_os9sender ) 

«endif 


/*  hangup  */ 

/*  interrupt  character  */ 

/*  kill  signal  from  Unix  */ 

/*  broken  pipe  from  other  host  */ 

/*  software  termination  */ 


r  Tt  »  *  «  *  1 


■*****! 


/*  Initialize  sender  **************** 


/*  Start  by  finding  default/desired  remote  host  to  connect  to 


*/ 
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server_entity  =  gethostbyname  ( remote_host_name) ; 
if  ( serve r_ent icy  ==  NULL) 

print! ( "os9sender  -remote  host  (\"%s\")  not  found\n", 

remote_host_naine)  ; 
exit  (-1 i ; 
} 
else  if  (TRACE) 

printf ( "os9sender  remote  host  (\"%s\")  located\n",  remote_host_name) ; 

/*  client  oDeri'^  server  Dort  **«**************************************«***/ 

/'  Fill  in  structure  ' server_address'  with  the  address  of  the  */ 

/*         remote  host  (i.e.  SERVER)  that  we  want  to  connect  with:      */ 

«i  f  defined (sqi ) 

Yjzero    ((char  * ':  &server_address  ,  sizeof  ( server_address )  )  ; 
<tend:  t 

i;erver_addres3.  sin_f  amily    =  AF_INET;       /*  Internet  protocol  family  •/ 

f   copy  server  IP  address  into  sockaddr_in  struct  server_address        */ 
*if  defined isgi ) 

bcopy  (server_entity->h_addr ,  & (server_address . sin_addr . s_addr) , 
server_entity->h_length) ; 
«?el3t- 

strncpy li (server_address . sin_addr . s_addr) ,  server_entity->h_addr , 
server_enti ty->h_lengch ) ; 
*end: : 

/-•   rriake  sure  port  is  in  networ)^  byte  order  */ 

strver_address.sin_port  =  htons  (Air\/£IM1_TCP_PORT_0 )  ; 

."  Open  TCP  (Internet  stream)  socket  */ 

if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

i 

printf  ("os9sender  client  can't  open  server  stream  socket"); 

exi t  ( -  1 /  ; 
\ 
else  if  (TRACE) 

printf  ("os9sender  client  opened  server  socket  successf ul ly \n" ) ; 

' '  Connect  to  the  server.   Process  will  block/sleep  until  connection  is 

is  established.   Timeout  will  return  an  error.  */ 

if  (connect  (  socket_descriptor, 

(struct  sockaddr  *)  &server_address, 

sizeof  (server_address) )  <  0) 

printf  ("os9sender  client  can't  connect  to  server  socket\n"); 
exit  (-1 ) ; 

else  if  (TRACE) 

printf ( "os9sender  client  connected  to  server  socket  successf ully\n" } ; 

}  /*  end  initialization  */ 

/'  loop  transferring  telemetry  until  shutdown_signal_received:  */ 

/****«**********************************************************♦**************/ 

/*  Two-way  reflector:   listen  to  local  prograin  and  relay  to  remote  host, 

listen  to   remote  host  and  relay  to  local  program  */ 

socket_stream  =  socket_descriptor;    /*  client  */ 

if  (TRACE) 
{ 

printf  ('os9sender  CLIENT:  socket_descriptor  =  %d,\n",  socket_descriptor) ; 
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pi'incf  ("  sockec_accepted   =  %d,\n",  socket_accepted)  ; 

pr^r.tf  i"  socket_s cream     =  %d\n",   socket_streain)  ; 

/'  c^j:  handshaKe.-  *  / 

write  (sockec_st ream,  "SUCCESS  #2:   os9sender  connected  to  os9server!",  46); 

read   (socket_st ream,  test_buffer,  46); 

re:-:'_bL:f  fer  [47  •  =  '  \n'  ; 

pr:ntf  ("test  handshake  between  hosts:  \n%s\n",  test_buf f er ) ; 

/♦■»*»*»♦«*♦*«*****♦♦««**»««**«**»***♦*«*«■***********«***************«********♦*/ 

/'  initialize  boolean  for  mam  loop  control  */ 
shutdown_signal_received  =  FALSE; 

while  ' shutdown_signal_received  ==  FALSE)   /*  loop  until  shutdown..  TRUE     */ 

{ 

/**»*»****««****«*****»*«*«««»**»*»**»******«**«******»*****«*********♦♦****/ 

/•    Sender   hiock  */ 

/"    read    from   local    stdm,    relay    to   remote   host  */ 

Ot^rs    !comjr.5nd_sent  I  ; 

rv-ces_received   =    strlen    (comm.and_sent )  ; 

if  'TRACE:  printf  ("los9sender  command_sent : %s ] \n" ,  command_sent ) ; 

::   tyte."_r-~eivec  •  0)   /*  read  failure  */ 

prmtf  '  "osF'sender  gets  i'  from  keyooard  unsuccessful  \n"  )  ; 
shutdown_cs9sender { j  ; 
} 

if  ■  by  tes_rece:\'ed  >  socket_length  ) 

prir.tf  '■  "039sender  send_telem€try_to_server  error:  "); 

pri.".tf  1  "E>ytes_received  toe  bia  for  packet  socket_length\n" )  ; 

print  f  i  "  "  '  ; 

printf  ( " 1 bytes_received=%d]  >  [socket_length=%d] ;  ", 

bytes_received,       socket_length) ; 

printf  ("string  truncatedXn" ) ; 
) 

bytes_left        =  socket_length; 

by  t  e  s_w  r 1 1 1  en     =  0 ; 

ptr  -  command_sent ; 

while  ((bytes_left  >  0)  ii  (bytes_written  >=  0))   /'  write  loop  ♦♦*••*«••»*/ 
{ 

bytes_sent  =  write  (socket_stream,  ptr,  bytes_left); 

if      (bytes_sent  <  0)  bytes_written  =  bytes_sent; 
else  if  (bytes_sent  >  0) 
{ 

bytes_left  -=  bytes_sent; 
bytes_written  +=  bytes_sent; 
ptr  +=  bytes_sent; 

} 
if  (TRACE) 

printf ( "os9sender  send_telemetry_to_server  loop  bytes  sent  =  %d\n", 
by tes_sent ) ; 
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if         (bytes_wricten   <   0) 

printf    ("os9sender  send_teleinetry_to_server    ()    send   failed,    " )  ; 
printf    ("%d  bytes_written\n" ,    bytes_written) ; 

else  if  (TRACE) 

printf  ("os9sender  send_telemetry_to_server  total  bytes  sent  =  %d\n", 

bytes_written) ; 

/*  Cneck  termination  */ 

if  istrncmp  (command_sent ,  "shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  ( shutdown_signai_received  ==  TRUE) 

?hutdown_Gs9sender  ( ) ; 
break; 

/WW************************************************************** 

/*  Receiver  block  */ 

/*****»**•*■»*«*****»»*♦*»*■»**»*******«***»********************«**************/ 

f    lister,  to  remote  host,  relay  to  local  network/program.  */ 

bytes_left        =  socket_length; 

by tes_received  =    0; 

ptr  =  command_received; 

while  ((bytes_left  >  0)  &i<  {by tes_received  >=  0))   /*  read  loop  ******»»**»»/ 
{ 

bytes_read  =  read  {socket_streami,  ptr,  bytes_left); 

if      (bytes_read  <  0)  bytes_received  =  bytes_read; 

else  if  (bytes_read  >   0) 

{ 

bytes_left  --    bytes_read; 

bytes_received   +=   bytes_read; 

ptr  +-  bytes_read; 

] 
if  (TPuZiCE) 

printf  ("os9sender  receiver  block  loop  bytes_read  =  %d\n", 
bytes_read ) ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  Iibytes_re3d  ==  0)  iti,  (bytes_received  ==  0))  break; 

if   {by tes_received  <  0)       /*  failure     */ 
{ 

if  (TRACE) 
{ 

printf  ("os9sender  receiver  block  read  failed,  "); 
printf  ( "bytes_received  =  %d\n",  bytes_received) ; 
} 
) 

else  if  (bytes_received  ==  0)   /*  no  transfer  */ 
( 

if  (TRACE) 

printf ( "os9sender  get_PDU_f rom_other_host  read  received  0  bytesXn'); 
} 
else  if  (bytes_received  >  0)   /*  success     */ 

printf  ("%s\n",  comniand_received)  ; 


276 


/■*  Check  terminacion  */ 

if      .-rrncm;".    ( corrmiand_received,     "shutdown",    8)    ==    0) 
shL.tdown_3igrial_received   =   TRUE; 

if  ( shucdown_s jgnal_received ) 
( 

shutdown_o£;9sender  (); 

bi  tak ; 
) 

;  /*  end  of  while  ( shutdown  =  =  FALSE)  indefinite  loop  */ 

3hjtdGwri_os9sender  i  )  ;        /  *  ensure  shutdown_os9sender  is  always  reached  */ 

print f  ("os9sender  exit  \n"); 

exit  (0);   /*  os9sender  complete  */ 
I 

' »    eriG    ?f    jTiai  r   prog  rail!  */ 

/"  Shutdown  block  */ 

void  3hutdown_os9sender  () 

if  .TRACE;  prmtf  ("os9sender  shutdown  in  progress  ...\n"); 

snutdown_sjgnai_received  =  TRUE;   /*  in  case  entry  was  from  signal  handler  */ 

/'  Nc  need  to  send  a  message  to  other  side  that  bridge  is  going  down,       ■•  / 
/'     si;ice  SIGPIPE  signal  trigger  may  shutdown_os9sender ( )  on  other  side?  */ 

if  (close  ':socket_stream)  ==  -1) 

v.rintf  :"os9sender  close  ;  socket_streami  j  failed\n"); 

if  (TRACE)  printf  ("os9sender  shutdown_os9sender  ()  complete\n " ) ; 

return ; 

)  /'  end  shutdown_os9sender  {)  •/ 
/********«****♦***«««**************************************************«««*****/ 
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C.  os9server.c  Unix  to  OS-9  Socket  Communications  Server 

/****«*******■*»***«*************»*«*»******************************************* 

Program:  os9server.c 

Description:       socket  test  program  to  pass  input  strings  in  2  directions, 

especially  socket  from  OS-9  auvsiml  to  Irix  auvsim4 


Revised: 

20  JAN  9- 

4 

Compilation : 

unix>  cc 

os9server.c  -o  os9 

os-9>  make  os9server 

Execution: 

hostl> 

os9server 

host2> 

os93ender  -r  hostl 

Example: 

fletch> 

os9server 

auvsiml>  os9sender  -r  fletch 

Execution  options: 

-h  -t  (first  letters  are  sufficient,  capital  letters  are  OK  too) 

-help  display  calling  syntax 

-trace  turn  on  TRACE  mode 

References:     !1)  (Gespac-provided)  EVIRA  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2)  Internetworking  with  TCP/IP  Volume  I:   Principles, 
Protocols  and  Architectures,  Douglas  E.  Comer, 
Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:   Design, 
Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 
(4i  IRIX  Network  Programmiing  Guide,  Silicon  Graphics  Inc. 
(5.  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 

Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 

Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 
(6)  Real-Time  Programming  Tutorial,  Bill  Mannel ,  SGI  Expo, 

Silicon  Graphics  Inc.,  23  May  93 
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Aucnor : 


Scarus : 


Don  Bruczman 

Code  OR/Br 

Naval  Postgraduate  School 

Monterey  CA  93945-5000 


brutzmanics .nps .navy .mil 

(408)  656-2149  work 
(408)  656-2595  fax 


Tested  satisfactory  Irix<==>Irix,  OS-9<==>OS-9 ,  Irix<==>0S-9 


Future  wor)< :       Consider  implementation  as  an  inetd  '  superserver '  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available, 

Comments,  suggestions  and  corrections  are  welcome! 


r  ******  1 


■****«***■ 


:**********! 


r**********i 


/  ■*  Irix  defines  in  /usr/include  */ 
*: f  defined (sgi ) 

o.nclude  <-stdio.h> 

♦  include  <-sys/ types  .h> 

♦  include  <sy3,'soc>;et  .h> 

♦  include  --net  inet /in  .  h> 
♦include  <netdb.h> 

♦  include  ^signal,  h-- 

♦  include  -string,  h'- 

/'  OS-9  subdirectories  /DEFS,  /DEFS/INET  '/ 

♦  el  3e 

♦  include   <-stdic.h> 
♦include   ■ types. h> 

♦  mriudtr   -'inet /'socket  .h- 
♦include   <inet/in.h> 

♦  include  --inet/netdb.  n> 
♦include  <signal.h> 
♦include  -errno.h> 


♦endif 

♦include  <time.h> 


/•  One  stream  socket  is  used  with  adequate  throughput 

/*    (although  two  could  work,  no  performance  improvement  is  expected) 

/*  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
/*    from  ether  processes  requesting  ports  on  your  system: 


♦define  DISBRIDGE  TCP  PORT 


♦define 

AUVSIMl 

TCP 

PORT  0 

3210 

/* 

♦define 

AUVSIMl. 

TCP 

PORT  1 

3211 

/* 

♦define 

AUVSIMl 

_TCP_ 

PORT  2 

3212 

/• 

♦define 

AUVSIMl 

TCP 

.P0RT_3 

3213 

/* 

♦define 

AlTVSIMl. 

TCP 

PORT  4 

3214 

/* 

♦define 

AUVSIMl. 

TCP 

PORT  5 

3215 

/* 

♦define 

AUVSIMl. 

TCP 

PORT  6 

3216 

/* 

♦define 

AUVSIMl. 

TCP 

PORT  7 

3217 

/* 

♦define 

AUVSIMl. 

TCP 

.P0RT_8 

3218 

/* 

♦define 

AUVSIMl. 

TCP 

.P0RT_9 

3219 

/' 

2056  /*  disbridge  program,  server  &  client     */ 

/*  NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 
/*     Underwater  Virtual  World  (UVW)     */ 


os9sender  <==>  os9server  test  programs  */ 
auv  execution  level  <==>  virtual  world  */ 
port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  */ 

port  for  future  use  •/ 
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*  define  TRUE   1 
«  define  FALSE  0 


#define  SOCKET_QUEUE_SIZE   5       /*  max  allowed  by  TCP/IP 


/*  function  prototypes  ♦**♦■ 


************************************************** 


/*  Note  that  function  prototype  parameters  may  need  to  be  deleted  if  the 

C  compiler  used  is  not  an  ANSI  compiler  (e.g.  OS-9  K&R  C  compiler)         */ 

void  shutdown_os9server  ( ) ; 

■"■  global  variable  definitions  *"■»****»»*********«**»«****•****»******»*•**»*»/ 

static  int  TRACE  -  0;  /*  1  =  trace  on,  0  =  trace  off  */ 

static  int  socket_descriptor, 

socket _accep ted, 

sock'^t_stream; 

static  int  socket_length  =  256; 

static  int  bytes_received,  bytes_read,  bytes_written, 

bytes_left,    bytes_sent; 

int  shutdown_signal_received; 

static  int  i  =  0,  ' 

print_help    -  FALSE; 

/*»«*♦****♦**«***«»*****♦**«•»*********************«***««********«**************/ 
/*»**********»******************«*****************«■****************************/ 

main  (argc,  argv; 

int  argc;  cnar  **argv;  /*  cominand  line  arguments  */ 

char  command_sent      [81], 

command_received  [81], 
remote_host_name  [50]; 

FILE  *netstat_f ileptr; 

struct  sockaddr_in       server_address; 

register  struct  hostent  *server_entity ; 

char  test_buffer  [81]; 

static  char  *ptr; 

/*  Begin  execution,  parse  command  line  «/ 

for  (i  =  1;  i  <  argc;  i++) 
i 

if  ((argv[i][0]  !=  '-')  II  print_help) 
( 

print_help  =  TRUE; 
break; 
} 
switch  {argv[i][l])    /*  switch  based  on  current  command  line  parameter  */ 
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case  ' h' :  /*  help  * / 

case  'H' :  /*  Help  '/ 

case  '  ?'  :  /■*    Help  */ 

prin:_i-ielp  =  TRUE; 

break ; 


cast  ' r ' : 
case  'T' : 

TPJ^CE  =  1  ; 

break; 

default : 
break ; 


/*  TRACE  feature 


/*  no  cominand  line  entry  parameter  present 


/*  end  switch  based  on  current  cominand  line  parameter  */ 
/*  end  for 


if  (print_help; 


/•  print  help  string  *' 


************************ 


printf  ("Usage:   os9server  [-help]  [-trace] \n" ) ; 
exit  (-1 : ; 
} 

if  (TRACE,  printf  1  " ios9server  TRACE  onl\n"i; 
f flush  (stdin); 

/■"  Command  line  parameter  parse  complete 


:i":::_33liZ':r   corrimuni  car  ions   blocks 


r*****»**Tt****' 


■****«*««**** 


Str"  sec r;et_^ enot h 
socKet_lengtn  -    81; 


/*  maximum,  allowed  packet  size  */ 


I:-^::a:ize  both  client  h.    server  »■'»**»»•♦''»»»»♦»•■■•««♦*•♦'»»*»»»«•**♦♦'«•♦'»»/ 

,'■•  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
/*    signal  handlers.   Otherwise  you  are  unable  to  ''C  kill  this  program.   */ 


*-f  defined :sgi ; 

signal  (SIGHUP,  shutdown_os9server) 

signal  (SIGINT,  shutdown_os9server ) 

signal  (SIGKILL,  shutdown_os9server ) 

signal  (SIGPIPE,  shutdown_os9server) 

signal  (SIGTERM,  snutdown_os9server ) 

*endi f 


/*  hangup  */ 

/*  interrupt  character  */ 

/*  kill  signal  from  Unix  */ 

/*  broken  pipe  from  other  host  */ 

/■*  software  termination  */ 


/*  Initialize  server 


■  **»****■ 


/*  setup  to  listen  for  client  to  attempt  connection 
\ 


/'  Server  opens  server  port 


******** 


■  *****■ 


■***********i 


/*  Open  TCP  (Internet  stream)  in  socket 

if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

{ 

printf  ("os9server  can't  'open'  stream  socket"); 

exit  (-1 ) ; 
} 
else  if  (TRACE) 

printf  ("os9server  socket  'open'  successfulXn" ) ; 


*/ 

*/ 
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/*  Bind  local  address  so  client  can  Calk  to  server  */ 

*if  defmedfsgi) 

bzero  ((char  *)  4<server_address,  sizeof  (server_address )  )  ; 
*endif 

server_address.sin_family      =  AF_INET;    /*  Internet  protocol  family  */ 

/'  make  sure  port  is  in  network  byte  order  */ 

server_address . sin_addr . s_addr  =  htonl  (INADDR_ANY) ; 
server_address.sin_port        =  htons  (AUVSIM1_TCP_PORT_0) ; 

if  (bind  {  socket_descriptor, 

(struct  sockaddr  *)  £<server_address, 

sizeof  (server_address) )  <  0) 
{ 

print f  .' "cs9server  socket  'bind'  unsuccessfulXn"  )  ; 
exi t  (  -  1 i  ; 
) 
else  if  (TRACE)  printf  ("os9server  socket  'bind'  successf ul\n" ) ; 

/'  prepare  socket  queue  for  connection  requests  using  listen  */ 

lister,     i.sockf  t_dei?criptor  ,    SOCKET_QUEUE_SIZE)  ; 
.f     iTRATEi 

print f i "os9server  socket  'listen'  successful  ...\n"); 


/  ■'  Server  'accept'  waits  for  client  connections  ****** 


r***1t************i 


if  (TRACE!  printf  ("osQserver  socket  waiting  to  'accept'  ...\n"); 
bytes_received  =  sizeof  ( socket_descriptor) ; 
wr.ile  ,  (socket_accepted  =  accept  (  socket_descriptor , 

Scserver_address, 

ibytes_receivedj  )  <  1)      /*  block  */ 
if  ; TRACE  J 
( 

printf  ("os9server  socket  'accept'  unsuccessful,  " ) ; 
printf  ("sleeping  1  second  ...\n"); 
sleep  ( 1 ;  ; 
) 
printf ( "os9server  connection  is  open  between  networks . \n" ) ; 

•  /'  end  initialization  */ 

/*««****»*«*»«**#**«*»««******«*****■»*«*«*««**«*»******«******************»*«**/ 

,■  ^    lOop  transferring  telemetry  until  shutdown_signal_received :  •/ 

/«»«*«*»*****«*»*****«*»****♦♦***********♦****************«******************«*/ 

/'  Two-way  reflector:   listen  to  local  program  and  relay  to  remote  host, 

listen  to   remiote  host   and  relay  to  local  program   */ 

socket_stream  =  socket_accepted;    /*  server  */ 

if  (TRACE) 
{ 

printf  ("os9server  SERVER:  socket_descriptor  =  %d,\n",  socket_descriptor) ; 

printf  ("  socket_accepted   =  %d,\n",  socket_accepted) ; 

printf  ("  socket_stream     =  %d\n",   socket_stream) ; 

} 

/*»***********««***««***********♦«*****«*********♦*********************«*****♦*/ 
/*  test  handshakes  */ 

write  (socket_stream,  "SUCCESS  #1:   os9server  connected  to  os9sender!",  46); 

read   (socket_stream,  cest_buf fer,  46) ; 

test_buffer  [47]  =  '\n'; 

printf  ("test  handshake  between  hosts:  \n%s\n",  test_buf fer) ; 


'****»******«*»******«*««**********«Tr**«*******i 


:«*«**«*♦»««»»»««««««»»»*«*/ 
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/*  initialize  boolean  for  main  loop  control  */ 
shutdown_signai_received  =  FALSE; 

while  ( shutdown_signal_received  =-    FALSE)      /*  loop  until  shutdown..  TRUE  */ 

'*■»»*****«**«**■*»***»********«*»***««****«******«*******«***«************«**/ 

/*  Receiver  block  */ 

/****«**«**»*****«******«**************»«**************************»******«■*/ 

/*  listen  to  remote  host,  relay  to  local  network/program  */ 

bytes_left        =  socket_length; 

by tes_received  =    0 ; 

p""  r  =  command_received; 

while  ! (byte£_left  >  0)  &&  {by tes_received  >=  0))   /*  read  loop  **♦*****•••*/ 
f 

byLts_read  =  read  i socket_stream,  ptr,  bytes_lef t ) ; 

if  (bytes_read  <      0)    bytes_received  =   bytes_read; 

else    if    ;bytes_read    >      0) 

bytes_left  -=   bytes_read; 

bytes_received   ■>■-   bytes_read; 
ptr  +=   bytes_read; 

} 
1 f     : TRACE ' 

printf  ("os9server  receiver  block  loop  bytes_read  =  %d\n", 
bytes_read; ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  ((bytes_read  ==  0)  &&  (bytes_received  =-   0))  break; 
} 

if   'byte3_received  •:  Oi       /*  failure     */ 

{ 

If  ;  TRACE ^ 
( 

printf  ("os9server  receiver  block  read  failed,  " ) ; 

printf  ( "bytes_received  -    %d\n",  bytes_received) ; 
1 
} 
el;:e  if  'bytes_received  ==  0)   Z*  no  transfer  */ 

if  (TRACE) 

printf  (  "os9sei-ver  get_PDU_f  romi_other_host  read  received  0  bytesXn"); 
) 
else  if  (bytes_received  >  0)   /■*  success     */ 

printf  ("%s\n",  command_received) ; 

/»»««*«*«*«♦*««««*»»»**»*»»»»««»«*«»**««»**»»»»«*««««*««**»»»»*««*««»««*«««»/ 

/*  Check  termination  •/ 

/**♦«»**•*♦♦«**♦**«♦♦**♦******♦»*******■*«****♦**«**«»««»**»*»«***♦**«««««*♦«/ 

if  (strncmp  (command_received,  'shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  (shutdown_signal_received  ==  TRUE) 
{ 

shutdown_os9server  ( ) ; 

break; 
) 
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/*  Sender  block  */ 

/»*»««««»««**«»»««*»*«««»*««*«»«»«««»«*»*«*««»«**»»«««»»♦*»«, «««»«««»»««««««y 

/*  read  from  local  stdin,  relay  to  remote  host  •/ 

gets  (command_sent ) ; 

bytes_received  =  strlen  (command_sent ) ; 

if  (TRACE)  printf  ("[os9server  command_sent : %s] \n" ,  comraand_sent ) ; 

if  (by tes_rece:ved  <  0)   /*  read  failure  */ 

< 

printf  ("os'3?erver  gets  ()  from  keyboard  unsuccessful\n"  )  ; 

shutdown_os9server  ( ) ; 
) 

if  (bytes_received  >  socket_length) 
( 

printf  ("os9server  send_telemetry_to_server  error:  " ) ; 

printf  ( "bytes_received  too  big  for  packet  socket_length\n" ) ; 

printf  ( "  "  ) ; 

printf  i " [byte?_received=%d]  >  ( socket_length=%d] ;  ", 
bytes_received,       socket_length) ; 

printf  '"string  truncatedXn" ) ; 
) 

bytes_left        =  socket_length; 

by tes_written     =  0 ; 

per  =  command_sent ; 

while    (  (bytes_lef t    >   0)    ii&    (by tes_written   >=    0))       /*   write   loop   ***«*******/ 

bytes_sent  =  write  (socket_stream,  ptr,  bytes_lef t ) ; 

it  (bytes_sent  <   0)  bytes_wri t ten  =  bytes_sent; 

else  if  (bytes_sent  >  0) 

bytes_left  -=  bytes_sent; 
bytes_written  +-  bytes_sent; 
ptr  +=  bytes_sent; 

} 
If  (TRACE' 

printf i "os9server  send_telemetry_tc_server  loop  bytes  sent  =  %d\n", 
by tes_sent ) ; 
I 

if    (by tes_written  <  0) 
{ 

printf  (''os9server  send_telemetry_to_server  ()  send  failed,  "); 
printf  ("%d  bytes_written\n" ,  bytes_written) ; 
) 
else  if  (TRACE) 

printf  (■'os9server  send_telemetry_to_server  total  bytes  sent  =  %d\n", 
bytes_written) ; 

/*   Check  termination  */ 

/*♦»*****♦**•♦*»***♦**»**«***»**»•»♦»*»♦*«**♦**********»«*»***«*»*«♦«»»««*««/ 

if  (strncmp  (command_sent ,  "shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  (shutdown_signal_received  --   TRUE) 
{ 

shutdown_os9server  (); 
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break; 

)  /*  end  of  while  (shutdown_signal_received==FALSE)  indefinite  loop  */ 

?:TU"dowr:_cs'^)3erver  i);        /*  ensure  shutdown_os9server  is  always  reached  */ 

print f  i"os9server  exit.Xn"); 

exit  (0);   /*  os9server  complete  */ 
1 

/  ■*  end  of  main  prograni  */ 

/'  Shutdown  block  */ 

void  shutdown_os9server  ( ) 

if  (TRACE'i  printf  ("os9server  shutdown  in  progress  ...\n"); 

shutdown_3ignal_received  -   TRUE;   /*  in  case  entry  was  from  signal  handler  */ 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down,      •/ 
/■*     since  SIGPIPE  signal  trigger  may  shutdown_os9sender  (  )  on  other  side?  */ 

if  'close  (SocKet_streaiTi)  ==  -1) 

printf  ;"os9server  close  ( socket_stream)  failed\n"); 

if  fTRATE'  printf  ("os9server  shutdown_os9server  ()  complete\n" ) ; 

return; 

j  /*  end  shutdown_cs9server  (;  */ 
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D.  disbridge.c  LAN  to  LAN  Connectivity  for  DIS  without  Multicast 

Prograr.  :  disbridge.c 

Description:  Relays  DIS  packets  between  LANs  on  different  Internet  hosts 

Revised:  7  July  93   DIS  version  1.3 

Compilation:  %  DIS-1.3/src>>  make  bridge 

Execution:  %  hostl>>  disbridge  -server 

%  host2>>  disbridge  [-client]  -remote  hostl . internet . address 

Execution  options: 

-s  -c  -r  -H  -I  -t  (first  letters  are  sufficient,  capital  letters  are  OK  too) 

-server  server  mode;  must  be  running  prior  to  client  calling  for  it 

[-client]   client  mode  [default);  remote  host  must  be  specified  and 
nave  disbridge  -server  already  running  there 

-remote  hostname   hostname . remote . net . address  for  client  to  connect  to  server 

-help   display  calling  syntax 

-interface  etO   will  force  use  of  a  given  device  interface  (e.g.  "etO")  for 
multicast  packet  listening  on  the  local  net.   disbridge  will 
attempt  to  open  the  first  known  device  if  the  default  device 
fails.   Only  one  parent  process  is  allowed  to  use  that 
device  on  a  given  machine.   Thus  if  you  want  to  connect  one 
local  area  network  (LAN)  with  another,  each  LAN  must 
dedicate  one  machine  to  disbridge.   This  appears  to  be  an 
unavoidable  hardware/multicast  restriction. 

-trace   turn  on  TRACE  mode 

References:     [1]    Military  Standard--Protocol  Data  Units  for  Entity 
Information  and  Entity  Interaction  in  a 
Distributed  Interactive  Simulation  (DIS), 
Institute  for  Simulation  and  Training,  University  of 
of  Central  Florida,  Orlando  FL,  30  OCT  91 

(2)  Internetworking  with  TCP/IP  Volume  I:   Principles, 
Protocols  and  Architectures,  Douglas  E.  Comer, 
Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:   Design, 
Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 
[A]    IRIX  Network  Programming  Guide,  Silicon  Graphics  Inc. 

(5)  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 
Samuel  J.  Leffier,  Robert  S.  Fabry,  Williairi  N.  Joy, 
Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 

(6)  Real-Time  Programming  Tutorial,  Bill  Mannel ,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 

(7)  DIS  vl.2  source  library,  John  Locke,  Naval  Postgraduate 
School,  Monterey  CA,  8  FEB  92 
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Thanks  to  John  Locke,  Mike  Macedonia  &  Dave  Pratt  for 

Tested  satisfactorily  for  EntityStatePDUs  using  vl.2  DIS 

Currently  transmits  EntityStatePDUs  after  removing 
parameters.   This  is  due  to  articulated  parameter 
not  surviving  transmission  of  the  PDU  image  over  the 

Testing  to  date  has  been  exclusively  on  SGI  wrkstations 

Irix  4.0.5E  operating  system 

Consider  implementation  as  an  inetd  ' superserver '  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available, 

Consider  conversion  froiti  iterative  to  concurrent  processing 
if  throughput  performance  needs  improvement. 

Initialize  opposite  hosts  using  AccivateRequest  & 

ActivateRepiy  exchanges,  as  well  as  DeActivateReguest  & 
DeActivateReply  exchanges. 

Comments,  suaaestions  and  corrections  are  welcome! 


■**»****«*******: 


r********* 


«:nclude  "disdefs.h"   /*  DIS  Library  */ 

^include  --n*^tdb.h-- 
Kinclude  --net  ^net  /  in  .  h-- 
#inciude  '-sys  'socKet  .  h> 
^include  -srdic .h  ■ 
^include  'time.h> 
# include  •■  sys 'types  .h> 

/*  One  stream  socket  is  used  with  adequate  throughput 

/*    (although  two  could  work,  no  performance  improvement  is  expected) 

/'  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
/*     from  other  processes  requesting  ports  on  your  system: 


♦define  DISBRIDGE  TCP  PORT 


2056  /*  disbridge  program,  server  &  client 


#def ine 

AUVSIMl. 

TCP 

PORT_0 

3210 

/* 
/* 
/* 

#def ine 

AUVSIMl. 

TCP 

P0RT_1 

3211 

/* 

♦define 

AlTVSIMl 

_TCP. 

.P0RT_2 

3212 

/* 

♦define 

AlTVSIMl 

TCP. 

PORT  3 

3213 

/* 

♦define 

AUVSIMl. 

TCP 

.P0RT_4 

3214 

/• 

♦define 

AUVSIMl. 

TCP 

P0RT_5 

3215 

/• 

♦define 

AUVSIMl. 

_TCP_ 

PORT  6 

3216 

/* 

♦define 

AlTVSIMl. 

_TCP. 

PORT  7 

3217 

/* 

Underwater  Virtual  World  (UVW) 
for  future  use 


NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 

*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
•/ 
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#defane   AUV£IM2_TCP_POP,T_8 
♦  define   AWSIMl    TCP    PORT   9 


3218  /* 

3219  f 


«det:ne  SOCKET_QUEUE_SIZE    5 
*d^rine  SOCKET_PADDING      10 


/*  max  allowed  by  TCP/IP  */ 

/*  a  little  extra  buffer  after  each  PDU   */ 


/*  function  prototypes  **••***** 


************************************************ 


/*  Note  that  these  function  prototypes  may  need  to  be  commented  out  it  the 

C  compiler  used  is  not  an  ANSI  compiler.  */ 

void  shutdown_disbridge  (const  int  status); 

void  send_PDU_to_other_host  (char  *send_PDU,  PDUType  send_type) ; 

int   get_PDU_fron-;_other_host  (char  **get_PDU,  PDUType  *get_type)  ; 

vo..d  pr:nr_PDU  (char      *pdu,  PDUType  type); 


************************ 


*************/ 
trace  on,  0  -    trace  off  */ 


/  *  global  variable  definitions  ******** 

static  int  TRACE  =  0; 

stat.r  int  socket_descriptor , 

soc)cet_accepted, 
socket_streaiTi; 

/'  maximun:  allowed  socket  length  value  =  ETHERMTU  [ /sys/netinet/i f ether  .h ]     */ 
/*  see  initialization  code  for  actual  value  */ 

static  mt 


static  ir.t 

s  t  a  1 1  I'  c  l"i  a  r 
static  PDUType 

static  char 
static  PDUType 

static  int 


socket_length  =  ETHERMTU; 

bytes_received; 

*local_PDU; 
local_PDU_type; 

*remote_PDU; 
remote_PDU_type; 


CLIENT, 
SERVER ; 


/*  boolean  variables  */ 


/*  John  Locke's  carried-over  variable  definitions  **«**«***«***«**«*»*»*' 

int         i  =0, 

print_help     =  FALSE, 

ethernet_flag  =  FALSE, 

nodes , 

read_f lag, 

recvd         =  0, 

sent  =  0, 

wri te_f lag; 

extern  unsigned  short   host_id; 

char 


PDUType 


*pdu, 
ethernet_interface  [MAX_INTERF+1] ; 

type; 
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rr.ain    iargc,    argv^ 

int    argc;    char    **argv;    /*    command   line   arguments    */ 
( 

char  coirjtiand    [81], 

new_device_name  [4], 
remote_host_nam6  [60]; 

inc  shutdown; 

FILE  *netstat_f ileptr; 

struct  sockaddr_in    server_address; 

regisrer  ?truct  hoster.r  *server_ent ity ; 

■-•.-,.-:,:■  test_buffer  [37]; 

/*  Beg:n  execution,  parse  command  line  •/ 


/ 1 


D»^fa'j2t  execution  i^  server  waiting  for  client  request  to  read.'write  PDUs   * ,' 

^,^-.\■vl.    -    TRUE; 
CLIE:JT  =  FALSE; 

for  ! :  =  1  ;  i  <-  arac;  i  +  +  ) 
{ 

if  !fargv[i][0]  '.-    '-')  II  print_help) 

print_help  =  TRUE; 
break  ,- 

switcn  largv : . J  [ i ] )    /'  switch  based  on  current  command  line  parameter  '/ 


case    'H' 
case  ' ? ' 


■■'  help  »/ 

/ '  Kelp  */ 

/'  He.p  '/ 
print_help  =  TRUE; 
break ; 

case  'i':      /*  Ethernet  interface  */ 
case  'I':      /'  Ethernet  Interface  */ 
e:hernet_f lag  =  TRUE; 
::  :i-:  ■-  argc  i 

if  (srrle;-.  ;argv[i-.l]  )  >  MAX_INTERF)  { 

printf!"%s:  error:  interface  name  exceeds  %d  charsXn", 

argv[0],  MAX_INTERF); 
exit  ;-l,  ; 
}  else  1 

strcpy  iethernet_interf ace,  argv[++i]); 

)  else 

print_help  =  TRUE; 
break ; 

case  ' C  :  /*   client  initiates  server  for  reading/writing  PDUs   */ 

case  ' C  : 

CLIENT  =  TRUE; 

SERVER  =  FALSE; 

break ; 

case  'r':      /*  remote  hostname  tells  client  who  to  connect  to     */ 
case  ' R' : 

CLIENT  =  TRUE; 

SERVER  =  FALSE; 

/*  get  remotehost  name  '/ 

if  (1-^1  <  argc)  strcpy  (remote_host_name,  argv[+  +  i]); 
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else  pririC_help  =  TRUE; 

break; 

case  's':       /*  server  waits  for  client  request  to  read/write  PDUs  */ 

CLIENT  :=  FALSE; 
SERVER  =  TRUE; 
break; 

case  't':       /*  TRACE  feature  */ 

case  'T' ; 

TRACE  =  1 ; 

break; 

default : 

print_help  =  TRUE; 
)  /*  end  switch  */ 
)     /*  end  for    */ 

if  (argc  ==  1  I |  print_help)  /»  print  help  string  «•*«*****»***»****•••*»»**/ 
i 

pr:ntf  ("Usage:   disbridge  [-client  -remote  hostname  I  -server  <default>] 

'  I  ; 

printf  ("  [-i  device*]  [-help]  [ -trace] \n" )  ; 

exit  (-1) ; 
) 

printf  (  "  \n" ) ; 

if  ( lethernet_flag;  strcpy  (ethernet_interf ace,  BCAST_INTERF) ;   /*  default  */ 

if  (net_open  (ethernet_interf ace)  ==  FALSE) 
i 

printf  ("disbridge  net_open  (\"%s\";  failed, \n",  ethernet_interf ace) ; 

printf  ("    due  to  improper  device  number  or  device  is  already  in  use\n"); 

net_close  ();   /*  clean  up  after  a  deficient  net_open  {)  DIS  function   */ 
/■"  this  recovery  procedure  would  be  better  located  there  */ 

/'  Show  user  what  correct  port  number  is  •/ 

if  (TRACE) 

{ 

sprintf  (command,  "netstat  -I"); 

printf   (         "netstat  -I\n"); 

system   (comimand);  /*  display  results  of  netstat  -I  command  */ 

} 

sprintf  (comimand,  "netstat  -I  >  netstat. I"); 
system   (command) ; 

if  ( (netstat_fileptr  =  fopen  ( "netstat . I " ,  "r"))  ==   (FILE  *)  NULL) 
{ 

fprintf  ("disbridge  failed  to  create  local  file  netstat . I \n" ) ; 

shutdown_disbridge  (1); 
) 

/•■  flush  first  line  then  read  first  device  name  from  file  netstat. I     */ 
while  (command  [0]  !=  '\n')  fscanf  (netstat_f ileptr ,  "%c",  &command [0] ) ; 
fscanf   (netstat_f ileptr,  "%3s",  new_device_name) ; 
fclose   (netstat_f ileptr) ; 
sprintf  (command,  "rm  netstat. I"); 
system   (command) ; 

/*  Fixed  problem  in  the  DIS  library,  bad  net_close()  call  caused  exit   */ 
strcpy  (ethernet_interf ace,  new_device_name) ; 
if  (net_open  (ethernet_interf ace)  ==  FALSE) 
( 

printf ( "disbridge  net_open  (\"%s\")  failed, \n",  ethernet_interface) ; 

printf ("    improper  device  number  or  device  already  in  use\n"); 

exit  (-1) ; 
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printf  ("disbridge  nec_open  (\"%s\")  succeededXn" ,  echerneC_interf ace) ; 

/'  ComiTianc  line  parameter  parse  complete  '/ 

/  ■*  Initialize  cominunications  blocks  */ 

/***«««**«**♦*****«****»***«***»#**«**«««*******«*«*»*****************«********/ 

/■•  set  socket_length  */ 

Z'  3ocket_length  =  ETHE?>1TU;  */  /*  maximuin  allowed  packet  size  */ 

socKet_lenqth  =  sizecf  i EntityStatePDU)  -  SOCK ET_P ADDING ; 
'*  Note  that  DI£  protocol  version  2  will  convert  'unused'  to  PDU  length  field  */ 

/***««***«*«»♦****«»»««*«*****»*****«*«***»*♦**«***************«»*******«»*»**»/ 
/*  Initialize  both  client  ^  server  »«»**«»«■'«»*♦»»*»»*»»«»»*»*«**»»»»***»**»«**/ 

/*  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
/'    signal  handlers.   Otherwise  you  are  unable  to  "C  kill  this  program.   */ 


signal  {SIGHUP,  shutdown_disbridge i 

signal  (SIGINT,  shutdown_disbridge) 

3iqr.?:l  (SIGKILL,  shutdowr._di  sbridge^ 

signal  'SIGPIPE,  shutdown_disbridge) 

signal  ,SIGTERM,  shutdown_disbridge) 


/*  hangup  */ 

/*  interrupt  character  */ 

/*  kill  signal  from  Unix  */ 

/*  broken  pipe  fromi  other  host  */ 

/•  software  termination  "/ 


/*  Future  work:   implement  inter-network  initialization  by  sending 

Send-ActivateRequestPDU' s  for  all  entities  on  the  local  net 
to  first  infcrrr,  the  remote  host  of  their  existence        */ 

/■'  n".t  implemented  -  PDU's  copied  when  received  regardless  of  activation   */ 

if  (CLIENT;   /■*  start  by  finding  remote  host  to  connect  to  */ 

server_enti  ty  =  aethostbynamie  i  remote_host_name)  ; 

,f   serve  r_er:'.ty  ==  NULL; 

print :'.  "disoridge  -remote  host  (\"%s\")  not  found\n", 

re!Tiote_host_name;  ; 
exit  {-1 : ; 
) 
else  if  (TRACE; 

printf  I  "disbridge  remote  host  (\"%s\")  located\n",  remote_host_name) ; 

/*  client  ODen*^  server  port  ************♦********************♦*«*«*******/ 

/*  Fill  in  structure  ' serve r_address'  with  the  address  of  the 

remote  host  (i.e.  SERVER)  that  we  want  to  connect  with  */ 

bzero  ((char  *)  &server_address,  sizeof  (server_address) ) ; 

server_address . sin_family   =  AF_INET;      /*  Internet  protocol  family  */ 

/*  copy  server  IP  address  into  sockaddr_in  struct  server_address        */ 
bcopy  (server_entity->h_addr,  & (server_address . sin_addr . s_addr) , 
server_entity->h_length) ; 

/*  make  sure  port  is  in  network  byte  order  */ 

server_address. sin_port  =  htons  (DISBRIDGE_TCP_PORT) ; 
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/*  Open  TCP  (Internet  stream)  socket  */ 

if  (  (socket_descriptor  =  socket  iAF_INET,  SOCK_STREAM,  0))  <  0  ) 

{ 

printf    ("disbridge   client    can't    open   server   streann   socket"); 

exit  (-1  ;•  ; 
i 
else  It    ; TRACE, 

printf  ("disbridge  client  open  server  socket  successful \n" ) ; 

/*  Connect  to  the  server.   Process  will  block/sleep  until  connection  is 

is  established.   Timeout  will  return  an  error.  */ 

if  (connect  (  socket_descriptor , 

(struct  sockaddr  *)  &server_address, 

sizeof  (server_address) )  <  0) 
( 

printf  ("disbridge  client  can't  connect  to  server  socket\n"); 
exi  t  ( -  1 ) ; 
) 
else  if  (TRACE) 

printf ( "disbridge  client  connect  to  server  socket  successful \n" ) ; 

■'  /*  end  if  (CLIENT)  initialization  '/ 

if  (£EFVER .   /"*  setup  to  listen  for  client  to  attempt  connection  */ 

/  •*    c  fi  i*ve  r  ODer*^  server  Dort  ****************************»«*«*********«***/ 

/■*  Cper.  TCP  .Internet  streax;  in  socket  */ 

if  (  'socker.descriptcr  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

printf    ("disbridge   server   can't    'open'    strearri  socket"); 
exit    (-1) ; 


r  .  .:tr  -  • 


printf  ("disbridge  server  socket  'open'  successful \n" ) 


.'••  Eind  local  address  so  client  can  talk  to  server  */ 

bzerc  ((char  ^i    &server_address,  sizeof  (server_address) ) ; 
server_address . sin_f amily      =  AF_INET;    /*  Internet  protocol  family  */ 
server_address.sin_addr.s_addr  =  htonl  (INADDR_A1JY)  ; 
servei_address.3in_port        =  htons  (DISBRIDGE_TCP_PORT) ; 

if  (bind  (  socket_descriptor , 

(struct  sockaddr  *)  &server_address , 

sizeof  (server_address ) )  <  0) 
{ 

printf ( "disbridge  server  socket  'bind'  unsuccessfulXn" ) ; 
exit  (-1 ) ; 
) 
else  if  (TRACE)  printf  ("disDridge  server  socket  'bind'  successful \n" ) ; 

/ ''   prepare  socket  queue  for  connection  requests  using  listen  */ 

listen  (socket_descriptor,  SOCKET_QUEUE_SIZE) ; 
if  (TRACE) 

printf ( "disbridge  server  socket  'listen'  successful  ...\n"); 

/*  Server  'accept'  waits  for  client  connections  ***************♦***♦***•*/ 

if  (TRACE)  printf  ("disbridge  server  socket  waiting  to  'accept'  ...\n"); 

bytes_received  =  sizeof  {socket_descriptor) ; 

while  ( (socket_accepted  =  accept  (  socket_descriptor, 

S(Server_address, 

&bytes_received) )  <  1)     /*  block  */ 
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if  (TRATE) 
1 

princf  ("disbridge  server  socket  'accept'  unsuccessful,  "  )  ; 

printf  ("sleeping  1  second  ...\n"); 

sleep  (1 ) ; 
) 
printf ' "disbridge  connection  is  open  between  networks . \n" ) ; 

)  /*  end  if  (SERVER)  initialization  */ 

/*«»«***»«******■«***************•*****»«♦*«**********«*****«*******«**»****»****/ 

/*  loop  until  shutdown:  */ 

/«»«**»**«»**»«»«»»*»**■»***«*»*«*»«*■*******♦**********«**********«*********««*«/ 

/'  Tv.'0-way  reflector:   listen  to  local  network  and  relay  to  remote  host, 

listen  to   remote  host   and  relay  to  local  network   */ 

if      (SERVER)     socket_stream  =  socket_accepted; 
el. 5^  /*  CLIENT  */   socket_stream  ~   socket_descriptor; 

::"  ^  TRACE; 

if  fCLIENT.i  printf  ("disbridge  CLIENT:   "  )  ; 

:f  (SERVER;  printf  ("disbridge  SERVER:   " )  ; 

printf (  "socket_descriptor  =  %d,\n",  socket_descriptor) ; 

printf ("  socket_accepted   =  %d,\n",  socket_accepted) ; 

printf ("  socket_stream     =  %d\n",   socket_stream) ; 

) 

/'  test  handshakes  */ 

if  'CLIENT- 

write  isccket_stream,  "disbridge  CLIENT  connected  to  SERVER",  36); 
if  (SERVER) 

write  (socket_stream,  "disbridge  SERVER  connected  to  CLIENT",  36); 
read  ( socKet_stream,  test_buffer,  36;; 
te3t_buffer  [37]  =  '\n'; 
printf  ("test  handshake  between  hosts:   %s\n",  test_buffer) ; 

/*  After  this  point  the  behaviors  of  the  CLIENT  and  SERVER  are  identical 

so  we  no  longer  bother  testing  for  which  version  is  being  executed   */ 

3hut:]own  =  FALSE;  /'  initialize  boolean  for  main  loop  control  */ 

while  ishutdowr.  --   FALSE;    /*  begin  to  loop  indefinitely  until  shutdown  met  */ 

/*  Sender  block  */ 

/»»»»»»»»♦««««»»«»«»»»»»»«»«»»»«»»»»*«»»»«»*««««♦««»*«*««»«««»»««««»*»««««»«' 

/*  read  local_PDU  from  local  network,  relay  it  to  remote  host  */ 

nodes  =  net_read  (ilocal_PDU,  &local_PDU_type) ;     /•  note  pointers  passed  */ 

if  (nodes  <  0)   /*  net_read  failure  */ 

( 

printf  ("disbridge  net_read  ()  of  local  PDU  traffic  unsuccessf ul\n" ) ; 
shutdown_disbridge  (1); 
) 

else  if  (nodes  >  0)   /*  at  least  one  PDU  found,  print  it  if  in  TRACE  mode  •/ 
( 

if  (TRACE) 
{ 

printf  ("disbridge  net_read  ()  local_PDU  captured  successfully : \n" ) ; 
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print_PDU  (local_PDU,  local_PDU_typei ; 
) 

/*  PDU  found.   No  need  to  check  origin  tag  prior  to  relaying  it  since 
DIS  litarai-y  will  ensure  that  net_read  ()  does  not  see  PDUs  that 
originated  from  the  self  process's  net_write  ().   Such  checks  are 
essential  at  some  level,  or  a  pathological  race  condition  will  develop 
and  possibly  swamp  both  networks  with  duplicated  PDU  traffic.        */ 

send_PDU_to_other_host  (local_PDU,  local_PDU_type) ; 
I 

c-^e  /'  nodes  ==  0,  no  pdu  found,  no  action  required  except  trace  */ 

if  (TRACE) 

printf  ("disbridge  net_read  ()  no  local  PDU  traffic  pending  ...\n"); 

/  ■*  Receiver  block  */ 

/*******************************«******«■**********«*********************«***/ 

/*  listen  to  remote  host,  relay  remote  PDU  to  local  network  */ 

/"  if  remote_?DU  found,  net_write  it  */ 

if  'get_PDU_f rom,_other_host  (Stremote_PDU,  &remote_PDU_type)  >  0) 
if    (TRACE  1 

printf  ("disbridge  get_PDU_f rom_other_host  successf ul : \n" ) ; 

print_PDU  (remote_PDU,  remote_PDU_type) ; 

printf  ("disbridge  calling  net_write  {)  " ) ; 

printf  ("to  issue  remote_PDU  locally  ...\n"); 
i 
if    ;net_write  (remote_PDU,  remote_PDU_type )  ==  FALSE) 

printf  ("disbridge  net_write  (remote_PDU)  unsuccessful \n" ) ; 

else   if  (TRACE) 

printf  '"disbridge  net_write  (remote_PDU)  successf ul \n ") ; 

else  if  ITRACE)  printf  ("disbridge  get_PDU_f rom_other_host  unsuccessful \n" ) ; 
/"  Check  termination  */ 

if  (shutdown)  shutdown_disbridge  ( 0 ) ; 

if  (TRACE) 
( 

printi 

sleep  (1 ) ; 
) 
/*  shutdown  =  TRUE;  */ 

)  /*  end  of  while  (shutdown==:FALSE)  indefinite  loop  */ 

shutdown_disbridge  (0);   /*  ensure  shutdown_disbridge  is  always  reached      */ 

exit  (0);   /*  disbridge  complete  */ 

I 

*/ 

■***************************«*****«*****************************»********/ 
************************************************************** 


/* 

■  *  *  ■ 

*****! 

•■******** 

/* 

*  *  *  *  * 

■  *  * 

*    *    *    «■    *    1 

********* 

/* 

end 

of 

main 

program 
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/*  shutdown  block  */ 

/***********************»**«****•*■**********************************************/ 

void  shutdowri_disbridge  (status) 

const  int  status; 
{ 

if  (TRACE)  printf  ("disbridge  shutdown  in  progress  ...\n"); 

/'  No  need  to  send  a  inessaqe  to  other  side  that  bridge  is  going  down, 

since  SIGPIPE  signal  triggers  shutdown_disbridge  ()  the  other  side    */ 

jf  (close  ( socket_stream)  ==  -l) 

printf  ("disbridge  close  ( socket_streani)  failed\n"); 

nec_close  ( ) ;  /"  close  local  net  port  connection  listening  for  DIS  PDU's    */ 
/*  watch  out  for  net_close  side  effects/interrupts  disabled!  */ 

prmrf  ("disbridge  exit.\n"); 

ex^r  (3tarusi;   /*  status  parameter  indicates  normal  or  abnormal  exit      */ 

»     /*    end    shiJtdrwr.    di.-bridae    (i     */ 


void  send_PDU_to_other_host   (send_PDU,  send_PDU_type) 

char  •send_PDU; 

PDUT>'pe        send_PDU_type; 

inr  i,  bytes_written,  bytes_left,  bytes_sent; 

char  *ptr; 

Enti tyStatePDU  *cast_PDU; 

char  PDU_character_buf fer  [ETKERMTU];   /*  ETHERMTU:  /sys/netinet /if ether . h  */ 

if  (send_PDu_type  ==  EntityStatePDU_Type) 
( 


'/ 


/*  prevent  passing  articulated  parameter  ponters 

cast_PDU  =  (EntityStatePDU  *)  send_PDU; 
cast_PDU->num_articulat_params  =  0; 

bccpy  '3end_PDU,  PDU_character_buf f er ,  socket_length ) ; 
if  (TRACE) 

pRintf  ("disbridge  send_PDU_to_other_host  socket_length  =  %d\n", 

socket_length ) ; 
printf  ("disbridge  send_PDU  type:   %d\n",  send_PDU_type) ; 
printf  ("disbridge  send_PDU  character_buf f er  =  [ " ) ; 
for  (i=0;  i  <  socket_length;  i++) 

printf  ("%x  ",  PDU_character_buf fer  [i]); 
printf  ( "  ] \n- ) ; 

printf  ("disbridge  send_PDU  character_buf f er  =  [ " ) ; 
for  (i=0;  i  <  socket_length;  i++) 

printf  ("%c",  PDU_character_buf fer  [i]); 
printf  (")\iT);  fflush  (stdout); 
} 

if  (socket_lenqth  >  ETHERMTU) 
{ 

printf  ("disbridge  send_PDU_to_other_host  error:  "); 

printf  ("PDU  too  big  for  packet"); 

printf  ("  ■); 

printf  ("[PDU  socket_length=%d]  >  [ETHERMTU=%d] ;  nothing  sentXn", 
socket_length,       ETHERMTU) ; 

return; 
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byLes_left  =    socket:_length; 

i; :  1  =  PDU_chaiaccer_buf  f  er  ; 

wiiile    ((byces_left     -   0)    &&     (by tes_wri tten   >=    0))  /*   write   loop   */ 

{ 

bytes_sent  =  write  (socket_stream,  ptr,  bytes_left); 

if       (bytes_sent  <   0)  bytes_written  =  bytes_sent; 

else  if  (bytes_sent  >   0) 

{ 

bytes_left  -=  bytes_sent; 
by tes_wri tten  +=  bytes_sent; 
ptr  +=  bytes_sent; 

) 
if  (TRACE) 

print f ( "disbridge  send_PDU_to_other_host  loop  bytes  sent  =  %d\n", 
Dytes_sent ) ; 
} 

if    {bytes_written  <=  0) 
{ 

printf  ("disbridge  send_PDU_to_other_host  ()  send  failed,  " ) ; 
printf  ("%d  by tes_wri ttenXn" ,  bytes_written ) ; 
i 

else    if  (TRACE) 
printf  ("disbridge  send_PDU_to_other_host  total  bytes_wri tten  =  %d\n", 
by tes_written) ; 
) 

eise  printf  ("disbridge  unsupported  PDUType  (%d),  not  passed  to  remoteXn", 
send_PDU_type) ; 

"!  /'  end  send_PDU_tc._other_host  ■;  */ 

int    get_PDU_f rom_otl-ier_host    (get_PDU,    get_PDU_type)     /*    returns    1    if    PDU   found   */ 

char    **get_PDU; 

PDUType  'get_PDU_type; 
i 

int  bytes_read,  bytes_left; 

char  *ptr; 

static   char  *reinote_PDU_buf  f  er  ; 

static    PDUHeader  *reinote_header ; 

remote_PDU_buf  f er  =  malloc  (soc)<et_length)  ; 

if    (reniote_PDU_buf  fer   ==   NULL) 

( 

printf 

("disbridge  get_PDU_f roin_other_host    remote_PDU_buf fer  malloc   errorXn'); 

return    (0);  /'    no   PDU   returned   */ 
) 

bytes_left  =    soc)^et_length; 

bytes_received  =   0; 

ptr  =    reinote_PDU_buf  fer; 

while  ( (bytes_left  >  0)  &&  (bytes_received  >=  0))  /*  read  loop  */ 

{ 

bytes_read  =  read  (socket_stream,  ptr,  bytes_lef t ) ; 

if  (bytes_read   <  0)    bytes_received   =    bytes_read; 

else  if  (bytes_read  >  0) 

{ 

bytes_left  -=  bytes_read; 
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by tes_received  +=  bytes_read; 
per  +=  bytes_read; 

if  (TRACE) 

princf  ("disbridge  get_PDU_f roin_ocher_host  loop  bytes_read  =  %d\n", 
by tes_read ; ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop 
if  ( (bytes_read  ==  0)  &&  {bytes_received  ==  0))  break; 


( by t e3_recei  ved  <•  0) 
if  (TRACE) 


/•  failure 


printf  ("disbridge  get_PDU_f  roin_other_host  read  failed,  "  )  ; 

prmtf  ( "bytes_received  =  %d\n",  bytes_received)  ; 
) 
return  (0);   /'  no  PDU  returned  '/ 


if  (tavtes  received 


o: 


/*  no  transfer  */ 


if  ( TRACE  1 

print f I "d-^bridge  jet_PDU_f rom_other_host  read  received  0  bytesXn"); 
return  iC'i;   /  *  no  PDU  returned  '/ 

*  else  I bytes_received  >  Oi   /*  success     */ 

*  Ca^z    and   convert    buffer    to   return    reiTiote_PDU   and    rernote_PDY_type  */ 

'PDUKeader 
iPDUType) 


rerTiott^_header 

••get_FL'J 

'get_PDU_type 


r  em  o  t  e_PDU_bu  f  f  e  r ; 
remote_PDU_bu  f  f  er ; 
reniote_header->type; 


if     ( TRACE  1 
{ 

printf  ( "disbridge  get_PDU_f  roin_other_host    read  bytes_received  =    %3d    \n", 

by tes_received; ; 
printf  ( "disbridge  get_PDU_f  roiT._other_host   read  socket_length   -   %3d   \n"  , 

socket_lengtri)  ; 
printf    ("disbridge   reniote_header  get_PDU_type :      %d\n",    *get_PDU_type) ; 
printf    ("disbridge  get_PDU      reiTiote_PDU_buf  fer   =    [ " )  ; 
for    ;i  =  0;    1    '-    3ocket_length;    l-^-^l 

printf    '"%x    ",    remote_PDU_buf fer    [i]); 
printf    (  "]  \n";  ; 

printf    ("disbridge  get_PDU    '.    reinote_PDU_buf  fer   =    [ "  )  ; 
f"r    (1=0;    i    --   socket_length;    i+-^i 

printf    ("%c",    reinote_PDU_buf fer    [i]); 
printf    ("l\n");    f flush    (stdout); 
ptr    =    ■*get_PDU; 

printf  ("disbridge  get_PDU  ;)  get_PDU  =  ["); 

for  (i  =  0;  i  '-  socket_length;  i  +  +) 

printf  ( "%c" ,  ptr  [i] ) ; 
printf  ("l\n");  f flush  (stdout); 
) 
return  (1);   /*  PDU  returned  */ 

/■•  end  get_PDU_f roni_other_host  ()  */ 


■********♦■ 


******************************* 


*************** 


/ 


void  print_PDU  (pdu,  type) 
char  *pdu ; 


PDUType  type; 


/•  from  John  Locke's  test_it.c  •/ 
/•  modified  to  stand  as  a  routine,  */ 
/*  no  other  changes  made  */ 


/•  example  PDUs      */ 
EntityStatePDU      *ESpdu; 
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FirePDU  *Fpdu; 

DetonationPDU  *Dpdu; 

ServiceRequestPDU  *SRpdu; 

ResupplyPDU  *Rpdu; 

ResupplyCancelPDU  *RCpdu; 

RepairCompletePDU  *RC_pdu; 

RepairResponsePDU  *RRpdu; 
CoiiisionPDU  *Cpdu; 

ActivacRequestPDU  *ARpdu; 

ActivatResponsePDU  *AR_pdu; 

EieaccivaCRequestPDU  *DRpdu; 
Deacc ivacResponsePDU         *DR_pdu; 

EmitCerPDU  *Epdu; 

RadarPDU  *Rad_pdu; 

ArticulatParamsNode  *APNptr;  /'  list  nodes  */ 

SupplyQcyNode  •SQNptr; 

RadarSysteiTi  *rad_sys_pcr; 

RadarSyscemNode  •RSN_ptr; 

RadarSystemsNode  *RSNptr; 

SroresNode  *SNptr; 

EniitcerNode  *ENptr; 

II  luminedEntityNode  *IENpcr; 

/*  Print  PDU  data  received  •/ 

if  (TRACE)  printf ( "disbridge  print_PDU  {)  PDU  Type  =  %d\n",  type); 

switch  (type) 

( 

case  !OtherPDU_Type) : 

print f I "Undefined  PDU  type  (OtherPDU_Type)  received . \n" ) ; 
break; 
case  (FirePDU_Type) : 

printf ("Got    %d: FirePDU\n"  ,    FirePDU_Type) ; 

Fpdu  =  {FirePDU  *)  pdu ; 
prlnt  f  (  "site  :  %d\n",  Fpdu->f iring_entity_id . address .site)  , • 

printf I "range :  %f\n",  Fpdu->range) ; 

break  ,- 
case  (EntityStatePDU_Type; : 

p  r  1  n  t  PE.'U  ',  i  c  h a  r  *  )  pdu  i  ; 

break ; 
case  (DetonationPDU_Type) : 

printf I "Got  %d : DetonationPDUxn" ,  DetonationPDU_Type) ; 

Dpdu  =  (DetonationPDU  *)  pdu ; 

break; 
case  (ServiceRequest PDU_Type) : 

printf ("Got  %d: ServiceRequestPDU\n" ,  ServiceRequestPDU_Type ) ; 

SRpdu  -    (ServiceRequestPDU  *)  pdu; 

printf ( "quantity  =  %f\n", 

SRpdu ->supply_qty_head->supply_quantity .quantity) 

break ; 
case  (ResupplyOf ferPDU_Type) : 

printf ("Got  %d : ResupplyOf ferPDUXn",  ResupplyOf ferPDU_Type) ; 

Rpdu  ~    (ResupplyPDU  *)  pdu; 

printf ( "of fer  quantity  =  %f\n", 

Rpdu ->supply_qty_head- >supply_quant  i  ty . quant  i  ty ) ; 

break ; 
case  (ResupplyReceivedPDU_Type) : 

printf ( "Got  %d:ResupplyReceivedPDU\n" ,  ResupplyReceivedPDU_Type) 

Rpdu  =  (ResupplyPDU  *)  pdu; 

printf ( "received  quantity  =  %f\n", 

Rpdu->supply_qty_head->supply_quantity .quantity) ; 

break; 
case  (ResupplyCancelPDU_Type) : 

printf ("Got  %d:ResupplyCancelPDU\n" ,  ResupplyCancelPDU_Type) ; 

break; 
case  (RepairCompletePDU_Type) : 

printf  ("Got  %d:RepairConipletePDU\n" ,  RepairCompletePDU_Type)  ; 
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break ; 
case  (RepairResponsePDU_Type; : 

prinCf("Got  %d:RepairRespon9ePDU\n" ,  RepairResponsePDU_Type) ; 

break ; 
case  (CollisionPDU_Type) : 

printf("Got  %d : Coll isionPDUXn" ,  CollisionPDU_Type) ; 

break ; 

/•  Experimental  PDU  Types  */ 
case  (ActivatRequescPDU_Type) : 

printf("Got  %d:AccivatRequestPDU\n" ,  ActivatRequestPDU_Type) ; 

ARpdu  =  (AccivatRequescPDU  •)  pdu  ,• 
prlnL  f  (  "radar_system  =  %d\n", 

ARpdu->radar_systems_head->radar_systems . radar_system) ; 

print f ( "stores  qty  =  %f\n",  ARpdu->stores_head->stores .quantity ) ; 

printf  I  "iri  =  %d\n" , 

ARpdu->articulat_params_head->articulat_parains.  ID)  ; 

break; 
case  (ActivatResponsePDU_Type) : 

printf ("Got  %d:ActivatResponsePDU\n" ,  ActivatResponsePDU_Type) ; 

break; 
case  (DeactivatRequestPDU_Type) : 

printf ( "Got  %d:DeactivatRequestPDU\n" ,  DeactivatRequestPDU_Type) ; 

break ; 
case  ;DeactivatResponsePDU_Type) : 

printf ("Got  %d:DeactivatResponsePDU\n" ,  DeactivatResponsePDU_Type ) ; 

break ; 
case  { Emitter PDU_Type) : 

printf  ("Got  %d  :  Eira  t  terPDU\n" ,  Emi  tterPDU_Type  )  ; 

Epdu  =  (Emi tterPDU  *)  pdu; 

printf ( "entity_type  =  %d\n",  Epdu->entity_type) ; 

print f ' "num_emitters  =  %d\n",  Epdu->num_emi tters) ; 

printf ( "emitter_params .param_l  =  %d\n", 

Epdu->eniitter_head->emitter  .emi  tter_pa rams  .param_l )  ; 

break ; 
case  (RadarPDU_Type) : 

printf ("Got  %d:RadarPDU\n" ,  RadarPDU_Type) ; 

Rad_pdu  =  (Radar PDU  *)  pdu; 

print f ( "location_to_entity .X  =  %f\n", 

Rad_pdu->radar_system_head->radar_system. location_to_entity .x) ; 

rad_sys_ptr  =  & {Rad_pdu->radar_system_head->radar_system) ; 

printf ( "target_id. address . site  =  %X\n", 

rad_sys_ptr->illumined_entity_head->illumined_entity . 

target_id. address .site) ; 

break ; 

default: 

printf (" Invalid  PDU  type  received . \n" ) ; 
break ; 

} 
)  /*  end  print_PDU  ( )  */ 
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Vin.  WORLD-WIDE  WEB  (WWW)  HYPERMEDIA  AND 
MULTICAST  BACKBONE  (MBone) 

A.  Introduction 

The  World-Wide  Web  (WWW)  project  has  been  defined  as  a  "wide-area 
hypermedia  information  retrieval  initiative  aiming  to  give  universal  access  to  a  large 
universe  of  documents"  (Hughes  94).   Fundamentally  the  WWW  combines  a  name 
space  consisting  of  any  information  store  available  on  the  Internet  with  a  broad  set  of 
retrieval  clients  and  servers,  all  of  which  can  be  connected  by  easily-defined  hypertext 
markup  language  (.html)  multimedia  links.   This  globally-accessible  combination  of 
media,  client  programs,  servers  and  hyperlinks  can  be  conveniently  utilized  by  humans 
or  autonomous  entities.   The  Web  has  fundamentally  shifted  the  nature  of  information 
storage,  access  and  retrieval  (Hughes  94)  (Bemers-Lee  94a,  94b). 

Universal  Resource  Locators  (URLs)  are  a  key  WWW  innovation.   A  block  of 
information  might  contain  text,  document,  image,  sound  clip,  video  clip,  executable 
program,  archived  dataset  or  arbitrary  stream.   If  that  block  of  information  exists  on 
the  Internet,  it  can  be  uniquely  identified  by  host  machine  IP  address,  publicly  visible 
local  directory,  local  file  name,  and  type  of  client  needed  for  retrieval  (such  as 
anonymous  ftp,  hypertext  browser  or  gopher).    Ordinarily  the  local  file  name  also 
includes  an  extension  which  identifies  the  media  type  (such  as  .ps  for  PostScript  file 
or  .rgb  for  an  image).   Thus  the  URL  completely  specifies  everything  needed  to 
retrieve  any  type  of  electronic  information  resource.   Example  URLs  appear  in  the  list 
of  references,  e.g.  (Hughes  94). 

The  Multicast  Backbone  (MBone)  permits  several-to-many  simultaneous 
high-bandwidth  communications  streams  across  the  Internet   Detailed  information  on 
MBone  applications  and  use  appears  in  (Macedonia,  Brutzman  94),   A  customized 
session  directory  (sd)  configuration  file  .sd.tcl  is  provided.   This  configuration  file 
adjusts  the  sd  interface  at  run  time  to  enable  automatic  DIS  connection  of  the 
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underwater  virtual  world  viewer  and  automatic  spawning  of  a  Mosaic  browser 
connection  to  the  NPS  AUV  WWW  Home  Page.   Thus,  once  a  system  has  been 
configured  using  free  software  for  Mosaic,  MBone  and  this  project,  connection  to  an 
already-running  underwater  virtual  world  requires  only  a  single  button  click  from  any 
compatible  workstation  on  the  Internet.   Global  connectivity  and  ease  of  use  are  thus 
demonstrated  in  order  to  further  encourage  remote  collaborative  research  and 
large-scale  widely-distributed  virtual  worlds. 
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B.  NPS  AUV  World-Wide  Web  Home  Page 

Hot  links  to  other  home  pages,  servers  or  media  are  portrayed  by  underlined 
text  in  the  home  page.  Line  breaks  and  page  layout  are  handled  automatically  by  the 
browser  according  to  window  and  font  size. 


NPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

ftp://taurus.cs.nps.navy.mil/pub/auv/auv.html 


NPS  Autonomous  Underwater  Vehicle  (AUV) 


^m  'i% 


(about  Say...) 


NPS  AUV  publication  abstracts  and  anonymous  ftp  server 
NPS  AUV  graphics  rendering  and  a  wireframe  rendering 

NPS  AUV  Underwater  Virtual  World 


finser  auv(a) dude. cs.nvs.navy.mil  for  a  sample  mission  report. 


A  free  tar  archive  of  the  virtual  world  software  is  here:  auv-uvw.tar.Z,  including 
source  code  and  executable  binaries  that  run  on  Silicon  Graphics  (SGI)  workstations. 

A  user  installation  guide  is  available.   Please  let  me  know  if  you  need  help  installing 
the  virtual  world.   Research  collaboration  is  welcome. 

Here  is  another  sample  NPS  AUV  mission  report  run  in  the  virtual  world. 
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Additional  supporting  papers  and  programs  used  with  the  virtual  world  software  are 
distributed  separately: 

•  "From  virtual  world  to  reality:  desisnins  an  autonomous  underwater 
robot"  paper  and  shdes. 

•  Vehicle  telemetry  postscript  plots  (20  pages)  for  the  SIGGRAPH 
mission. 

•  Mission  script  syntax  for  NPS  AUV  execution  level  control: 
mission. script-HELP  file. 

•  www  line  mode  World-Wide  Web  browser  distribution  page  for  the 
www  anonymous  ftp  directory,    www  is  used  to  make  World-Wide 
Web  queries  from  inside  the  virtual  world.   This  program  is  needed  if 
you  want  live  text  to  speech  capability.    Put  www  in  the  /dynamics 
directory  or  have  your  system  administrator  install  it. 

•  gnuplot  plotting  program  distribution  directory  (and  FAQ  page). 
gnuplot  is  used  to  plot  robot  telemetry  results.   This  program  is 
needed  if  you  want  a  plotting  capability.   Put  it  in  the  /execution 
directory  or  have  your  system  administrator  install  it. 

•  MBone  Multicast  Backbone  connection  information.   An  MBone 
connection  is  needed  if  you  want  to  participate  in  worldwide 
audio/video/DIS  multicasts  with  the  underwater  virtual  world.   If  you 
are  a  local  user  on  the  gravy  cs.nps.navy.mil  subnet,  you  don't  have 
to  download  the  full  distribution  but  instead  can  copy  the  MBone 
session  directory  (sd )  configuration  file  .sd.tcl  and  .mailcap  mosaic 
initialization  file  to  your  root  directory,  and  then  paste  the  following 
.cshrc  aliases  into  your  root  directory  .cshrc  file.   After  you  source 
.cshrc  you  are  ready  to  run  sd  and  mosaic. 


303 


1       ^ 

i'f  1 

%JilKKM          "  '"'^^'X, 

The  NPS  AUV  Underwater  Virtual  World  also  appeared  at  The  Edge  exhibition  at 
SIGGRAPH  94,  July  26-29  in  Orlando  Florida  USA.   There  was  a  simultaneous 
MBone  multicast  on  the  worldwide  Internet  all  that  week.   Information  resources  from 
that  exhibit  include: 


project  abstract, 

collaborator  hst, 

people  pages, 

1000- word  description  and  proposal  for  exhibition  in 

The  Edge  exhibition  at  SIGGRAPH  94,  and 

information  regarding  the  audio/video  multicast  on  the  Internet 

MBone. 


The  Universal  Resoiu-ce  Locator  (URL)  for  this  home  page  is 
ftp:lltaurus.cs.nps.navy.millpublauvlauv.html 
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C.  Hypertext  Markup  Language  (HTML)  Syntax  Summary 

The  following  HTML  syntax  summary  will  help  decipher  most  of  the  home 
page  document  which  follows  in  the  next  section.   The  key  feature  of  HTML  is  that  it 
describes  document  structure:    images,  the  relative  emphasis  of  text  entries  and  links 
to  other  documents  or  files  via  URLs  (NCSA  94b). 

The  rendered  format  of  an  HTML  document  can  vary  depending  on  the 
browser  used  to  access  it.    This  permits  both  graphics  terminals  and  character-based 
terminals  to  each  access  and  display  a  document,  preserving  contextual  links 
throughout.    URL  media  format  is  indicated  by  the  filename  extension.   The  ability  to 
display  movies,  play  sounds,  and  utilize  other  media  depends  on  the  capabilities  of  the 
connecting  machine,  the  presence  of  'viewer'  programs  which  can  receive  and  output 
the  media  after  a  handoff  from  the  browser,  and  proper  links  between  MIME  types 
and  browsers  as  configured  in  the  user's  .mailcap  file. 
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The  following  is  a  short  synopsis  of  key  HTML  markups.  Markup  tags  are 
generally  case  insensitive.  Blanks  and  skipped  lines  between  markup  tags  have 
no  effect. 

See  URL  http://www.ncsa.uiuc.edu/General/Internet/WWW/HTMLPrimer.html 
for  detailed  information  on  HTML  syntax. 


<B>  bold  text  </B> 

<I>  italics  text  </!> 

<P>  inserts  a  paragraph  break. 

<HR>  places  a  horizontal  rule  before  the  next  item: 

<A  IMG=" universal  resource  locator"> 

places  an  inline  image  on  the  page. 

<A  HREF=" universal  resource  locator">  button  text> 

places  button  text  on  the  screen  which  retrieves  "universal  resource 
locator"  when  selected.   Inline  images  can  be  used  in  place  of  text. 

<ul>  starts  an  unnumbered  list 

<li>  starts  a  list  item 

<li>  starts  another  list  item 

</ul>  ends  an  unnumbered  list 

Figure  5.    Hypertext  Markup  Language  (HTTVIL)  Syntax  Summary. 
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D.  auv.html  NPS  AUV  Home  Page  (Hypertext  Markup  Language) 

v-HEADER- 

<TITLE>  NPS  Auconomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page</TITLE> 

.--/HEADER> 

<--m~'  <A  NAME=AU\'>  NPS  Auconomous  Underwater  Vehicle  (AUV)  </Hl> 

<A  HREF=  "  f  tp  :  //taurus  .  cs  .  nps  .navy  .mil/pub/auv/auv  .  iv.gif  "xIMG 
SRC=" ftp: //taurus .cs . nps . navy .mil /pub/auv/auv. iv.gif"  ALT="NPS  AUV  logo"></A> 

<A  HREF= "ftp :/ /taurus . cs .nps .navy .mil /pub/auv/nps_auv . au"><IMG 
SRe'= " ftp: // taurus .cs. nps . navy .mil /pub/mosaic /sound. xbm"  ALT=" sound  icon"></A> . 

<A 
HREF^-nttp: / /www_tios . cs . utwente . nl : 8001 /say/?Naval + Pos t gradu a te* School , Autonomous +Und 
(riwateL  -^Ve;iicle">-.  IMG  SRC="ftp://taurus.cs.nps.  navy  .mil  /pub/mosaic /sound,  xbm" 
ALT=" sound  icon"></A> 

(about  <A  HREF  =  "http://www_tios.cs.utwente.nl/say/?">  Say...</A>) 

<K2>     NPS  Ab'V  <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/AUVPAPERS"> 
publication  abstracts</A>  and 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv">  anonymous  ftp 
server<i">  '  ftp  :/  /taurus  .  cs  .nps  .navy  .mi  1 /pub/auv)  </i></A>   </H2> 

<B>  NPS  AUV  </B>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/auv.iv.rgb.Z"> 
Graphics  rendering</A>  and  a 

<A  HREF  = 
" ftp: // taurus .cs .nps . nav^'  .mil /pub/auv/auv. i v. wireframe. rgb. Z">  wireframe  render ing</A> 
■-P> 

•■hr> 

<H1>     <A  NAME="AUV-UVW">  NPS  AUV  Underwater  Virtual  World</Hl> 

<A  HREF  =  "ftp: //taurus . cs .nps .navy .mil/pub/auv/auv-uvw. gif"><IMG  SRC  = 
"ftp://taurus.cs.nps.navy.mil/pub/auv/auv-uvw.gif"  ALT="AUV  Underwater  Virtual  World 
ir.age"  .".-/A  •  •:P> 

Finger  the  NPS  AUV  account  <A  HREF  = 
"http : / /www .cs. Indiana. edu /finger /gateway ?auv@dude. cs .nps . nav^'  .mil '><i> (finger 
auv(adude  .  cs  .  nps  .  navy  .mi  1 )  </i></A>  for  a  sample  mission  report. <P> 

<-B>    -'A  HREF  -  "ftp  : //taurus  .  cs  .nps  .navy  .mil/pub/auv/auv-uvw.  tar .  Z">A  free  tar 
arcnive  of  cne  virtual  world  software  is  here:  auv-uvw. tar . Z</A>  </B> 

including  source  code  and  executable  binaries  that  run  on  Silicon  Graphics 
'?GIi  worksta  ti  ons  .  <P'' 

A  <A  HREF  = 
"ftp://taurus.cs. nps. navy. mil /pub /auv/ auv-uvw .virtual -world . INSTALL •>user  installation 
guide</A> 

is  available.   Please  let  me  know  if  you  need  help  installing  the  virtual 
world.   Research  collaboration  is  welcome.  <P> 

Here  is  another  sample  <A  HREF  = 
•ftp://taurus.cs. nps. navy . mi 1 /pub / auv / auv-uvw. virtual -world. README ">NPS  AUV  mission 
report</A>  run  in  the  virtual  world.  <P> 

Additional  supporting  papers  and  programs  used  with  the  virtual  world 
softare  are  distributed  separately: 
<ul> 

'-li>   <A  HREF  =  "ftp: //taurus. cs .nps .navy .mil/pub/auv/aaai92ws .ps.Z'><i>*From 
virtual  world  to  reality:  designing  an  autonomous  underwater  robot"</i>  paper</A>  and 

<A  HREF  = 
•  ftp: //taurus .cs . nps .navy .mil/pub/auv/aaai92ws . slides .ps. Z">slides</A>. 
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-li  •    <A  HREF  3 
"  ftp:  /  ,'taurus  .C3  .nps  .  navy  .mil  /pub/a  uv/AUV_t  el  emetry  .ps  .  Z"><B>VehiclG  teleinetry</B> 
postscript  plots  (20  pages)  for  the  SIGGRAPH  niission</A>. 

■  1-     A  HREF  =  "  ftp  : //taurus  .  cs  .nps  .na\ry' .mil/pub/auv/mission.  script  .HELP"> 
Missior.  script  syiitax  for  NFS  AUV  execution  level  control</A>:  mission,  script  .HELP 

filt. 

<ll>    <A  HREF  = 
"http : / /inf o. cern . ch /hypertext /WWW/LineMode/Defaults/Di stribution.html "><B>www</B> 
line  mode  World-Wide  Web  browser</A>  distribution  page  for  the 

<A  HREF  -    "ftp : //inf o. cern .ch/pub/www/src/ "><B>www</B>  anonymous  ftp 
directory</A> . 

<b>www</b>  is  used  to  make  World-Wide  Web  queries  from  inside  the  virtual 
AOi\.d.   This  program  is  needed  if  you  want  live  text  to  speech  capability. 

Put  •^'b'.-'Www-  /b>  in  the  /dynamics  directory  or  have  your  system  administrator 

-]";S  tai  i  It. 

-rii-   <A  HREF  =  "http://www.cs.dartmouth.edu/gnuplot/"> 
-  B -gnupiot-  /B  •  plotting  program</A>  distribution  directory  (and 

-A  HREF  -    "ftp://ftp.dartmouth.edu/pub/gnuplot/faq/gpt_faq.html">  FAQ 
psge-  /A-  . 

-'b -qnuplotv/b--  is  used  to  plot  robot  telemetry  results.   This  program  is 
r.rr-d^d  -f  \'ou  want  a  plotting  capability. 

Put  it  .n  the  /execution  directory  or  have  your  system  administrator 
.nstall  it . 

<li,-   <A  HREF  =  "ftp : //taurus . cs .nps .navy  .mil/pub/mosaic/mbone. html "><B>MBone</B> 
Multicast  &ackbone</A->  connection  information. 

.An  MBci-it  connection  is  needed  if  you  want  to  participate  in  worldwide 
audio,'Vj.deo,  DIS  multicasts  with  the  underwater  virtual  world.   <P> 

If  you  are  a  local  user  on  the  grav^'l  .  cs  .nps .  na^'y  .mil  subnet,  you  don't 
have  tc  download  the  full  distribution  but  instead  can  copy  the  MBone  session  director 
•.-b>si:-",  D>;  conf igiiration  file 

--A  HREF  -  "ftp://taurus.cs.nps.navy.mi1/pub/auv/.sd.tcl"> 
-■£.^  .  sd  .  tci-  /& '</A'--  and 

vA  HREF  =  "  f  tp  :  /  /  taurus  .  cs  .nps  .na■^^y  .mil/pub/auv/ .mailcap"><B>.mailcap</B> 
niosaic  initialization  file</A>  to  your  root  directory,  and  then  paste  the  following 

>.A  HREF  = 
"  ftp  :// taurus . cs . nps . navy .mil /pub/auv/ . cshrc- excerpt "><B> . cshrc</B>  aliases</A>  into 
your  root  directory  <B> .cshrc</B>  file.    After  you  <B>source  .cshrc</b>  you  are  ready 
to  run  <D>sd</b>  and  <b>miosaic</b> .  <P> 

</ul:- 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/pool-search.gif">  <IMG  SRC 
=  "ftp://taurus.cs.nps.navy.mil/pub/auv/pool-search.gif"  ALT="AUV  pool  search 
image"  ••  /A><P> 

The  <B>NPS  AUV  Underwater  Virtual  World</B>  also  appeared  at 
<A  HREF  =  "http://www.vsl.ist.ucf.edu/projects/edge/edge.html"> 
<B>The  Edge</B></A>  exhibition  at 

■-A  HREF  =  "gopner : //siggraph .org/00/conf erences/siggraph94/gip/gip. txt ■> 
-••E--  SIGGRAPH  94<-''B></A>,  July  26-29  in  Orlando  Florida  USA. 
There  was  a  simultaneous 
-.A  HREF  = 
•ftp://taurus .cs. nps. navy .mil/pub/auv/SIGGRAPH94/SIGGRAPH94-mbone .html ■>  MBone 
multicast  on  the  worldwide  Internet</A>  all  that  week. 

Information  resources  from  that  exhibit  include: 
<ul> 
<li>    <A  HREF  =  "ftp: //taurus.cs .nps. navy .mil/pub/auv/SIGGRAPH94/abstract . txt"> 

project  abstract</A>, 
<ii>    <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/SIGGRAPH94/contacts.txt"> 

collaborator  list</A>, 
<li>    <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/pratts/home.html"> 

people  pages</A>, 
<li->     <A  HREF  = 
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"f rp: //taurus .cs.nps.navy .mil /pub/auv/SIGGRAPH9  4/edge_description . txC ">  1000 -word 
de5cripciori-;/A>  and 

<A  HREF  = 
" icp : //taurus .cs . nps . navy .mil /pub/auv/SIGGRAPH9  4/edge_proposal .ps . Z">   proposal</A> 
for  exhibition  m 

-i:  •    <A  HREF  =  "http://www.vsl.ist.ucf.edu/projects/edge/edge.html"> 
<B>The  Edge^/B></A>  exhibition  at 

<A  HREF  =  "gopher://siggraph.org/00/conferences/siggraph94/gip/gip.txt"> 
<B>  SIGGRAPH  94</B></A>,  and 
<li~-    infonriation  regarding  the  <A  HREF  = 
"ftp: //taurus .cs.nps. navy  .mil /pub/auv/SIGGRAPH94/SIGGRAPH94-mbone . html ">audio /video 
mule  cast  on  the  Internee  MBone</A>. 

■:nr  • 

<H1  •    IEEE  Oceanic  Engineering  Society  AUV  94  Conf erence</Hl> 

■rA  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/auv94.logo.gif">  <IMG  SRC  = 
"  ftp: // taurus  .cs  .  nps  .  na^vT/ .mil /pub/auv/auv94  .  logo. gif"  ALT="AUV  94  logo"></A>  <P> 

AU^  94  took  place  July  19-20  1994  in  Cambridge  Massachusetts  USA. 

<ui> 

<li->    <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/auv94.program.txt"> 
Conference  Program  in  text</A>  and 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/auv94.ps.Z"> 
pcstscript</A>  formats, 

<li>    <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/auv94video.info"> 
Video  Proceedirjgs  4.  broadcast  inf  ormation</A> ,  and  the 

<'li>    <A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/auv94video.ps.Z"> 
Videc  Proceedings  abstract  bookiet</A>. 

We  were  able  to  broadcast  the  video  proceedings  and  selected  sessions  over 
tne  MBone. 

Conference  e-mail  queries  may  be  sent  to  <i>auv94@ieee . org</i>   <P> 

<hr> 

<H2>   Otner  good  stuff!  </H2> 

'•A  HREF  =  "  ftp  :// ftp  .  nps  .  na\/"y  .mil/pub/usw/av_mcm  .html  •>  <B>Symposium  on 
Autonomous  Systems  m  Mine  Countermeasures</B></A>  at  NPS  April  4-6  1995.   <P> 

Unmanned  Untethered  Submersible  Technology  (UUST)  93  <A  HREF  = 
"ftp: //taurus.cs. nps. navy .mil /pub/auv/uust93_video_order . f orm">  video  proceedings 
aostracti--  /A>  witn  order  fonri  <P> 

<A  HREF  =  "http://www.cs.wisc.edu/~pruyne/os9faq.html">  <B>0S-9 
FA'.-  '&■•  /A-:   the  OS-9  real-time  operating  system  is  what  we  use  in  the  vehicle. 

Also  pertaining  to  OS-9  is 

<A  HREF  =  "http://igiou.com/~stevenw/windsorhome.html">  Windsor  Systems 
Home  Paqe</A>.   <P> 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/healey.rgb.Z">  Tony 
Healey</A>  (healey@lex.me.nps.navy.mil) , 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/mcghee.rgb.Z'>  Bob 
McGhee</A>  (mcghee@cs.nps.navy.mil),  and 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/brutzman.rgb.Z">  Don 
Brutzman-'/A>  (brutzman@nps  .  navy  .mil ) 

<-A  HREF  =  "http://www.cs.indiana.edU/finger/gateway7brutzman@nps.navy.mil"> 
contact  info</A>  <P> 

return  to  <A  HREF=" ftp: //taurus .cs .nps . navy .mil /pub/mosaic/nps_mosaic .html#TOC"> 
NPS  Home  Page  table  of  contents</A> 

-'/ul> 
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<B>   f  tp  : //taurus.cs  .nps  .navy- .r.  -    _r_c  auv/auv.htrl    <   5>  <?> 

a:  :f ££?:■•  NPS  AU\'  Wcrld-Wide-Web  Heme  Page:    <A  KP.EF  = 

"iij;:      rsuiu?  . c?.r.p?  .na\y -mil   put   auv  trutzmar..  rgb.  Z*>  Don  BruC2rr,an</A> 

(bruczmaniriDS  .  nav^- . mi  i ) 

<A   HREF    = 
■  he cp :/ /wwiv-. cs.  indiar.a  .  ed-   rir.jer   C5:ew5y?i:r_rznian@nps  .  r.a'.y  .irdl  •>  conract   info<.A>    (18 
October   94! 
-■/APDRESS> 
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E.  .sd.tcl  Multicast  Backbone  (MBone)  Configuration  File 

#  SHeade:  :  sd_start  .  tcl ,  v  1.4  (Van  Jacobsor.  LBL) 
« 

«  NOTE:        This  file  was  modified  froiTi  the  default  .sd.tcl 

#  to  support  automatic  execution  of  multicast  DIS 

#  virtual  world  programs  for  the  NPS  AUV  and  NPSNET, 

#  and  to  automatically  spawn  mosaic  when  a  URL  is 

#  present  in  the  session  advertisement. 
« 

#  Please  report  additions,  comments  and  corrections  to 

#  Don  Erjtzman,  brutzman@nps.navy.mil 
« 

«       last  modified:   19  OCT  94 
« 

#  URL  of  this  example  file  is: 

#  f  tp:  /  /taurus  .  cs  .nps  .nav^'  .mil/pub.'auv/  .  sd.  tcl .  auv-uvw 

«  tcl  'hocks'  invoked  when  sd  takes  some  action  on  a  session. 

«  .50  will  invoke: 

#  start_se3sicn  when  the  user  asks  to  'open'  (start)  a  session 
«       create_s6ssion  just  after  the  user  creates  a  new  session 

#  heard_sessicn  when  announcement  for  a  session  is  first  heard 

#  deiete_session  when  the  user  or  a  timeout  deletes  a  session 
« 

#  When  a.ny  of  the  above  are  invoked,  the  global  array  sd_sess 

#  contains  all  the  information  about  the  session  to  be  started: 

#  SG_sess ;name ; 

#  sd_ses£ 'descript i on } 

#  sci_sess  .'address  , 

#  sd_sess : ttl : 

#  sd_3e£s  creator • 

«  £d_se?s :creator_id; 

#  sd_sess (source_id; 

#  sc_sess !arrival_time; 

#  sd_sess ( start_time ■ 

#  sd_sess 1 end_t ime . 

»   sd_3ess i attributes;  (list  of  session  attributes) 

»   sd_3ess 'med.a ,  ilist  of  media  names) 

#  For  each  media  na.T.ie-  there  is  an  array  containing  the  information 

#  about  that  media: 
«   sd_$media :port i 

it   sd_$media  iconf_id) 

#  3i_rmedia (attributes)       (list  of  media  attributes) 
« 

#  Media  and  session  attributes  are  strings  of  the  form  "nanie"  or 

#  "  r<ame  :  va  j.  ue  "  . 
# 

#  Some  global  state  information  is  available  in  array  sd_priv: 

#  sd_priv (audio)  (=  0  if  audio  disabled  with  -a) 

#  sd_priv (video)  (=  0  if  video  disabled  with  -v) 

#  sd_priv (whiteboard)  (=  0  if  wb  disabled  with  -w) 

proc  start_se3sion  {}  { 

global  sd_sess  sd_priv  mosaic 

«  thanks  to  Bill  Fenner  of  Navy  Research  Lab  for  posting  this  addition. 

«  start  up  Mosaic  if  there  is  a  URL  in  the  description. 

if  { [ regexp  { [a-zA-Z] + : // (^  ]+}  $sd_sess (description)  url]}  { 

exec  Smosaic  -home  Surl  & 
) 

#  invoke  the  appropriate  start  proc  for  each  of  the  media 
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4t    i  f    such    a   proc   exists   and   chat    media    is   enabled. 
foreach  in   Ssd_sess  imedia)     { 

if    {     [Ilengch    [info   proc   start_Sm]  )    &£<    $sd_priv($m)     }     { 
scart_$in 

) 


proc  st:art_audio  {  )  ( 

global  sd_sess  sd_audio 
sec  audiofmc  " " 
sec  packeCfmc  "-n" 
foreach  a  $sd_audio (attribuces)  { 
case  $a  { 

f mc : *  {  sec  audiofmc  (string  range  $a  4  end]  ) 
vc   (  set  packeCfmc  "-v"  ) 
) 
} 
sec  confaddr  (formac  "%s/%s/%s/%s/%s"  $sd_sess ( address)  \ 

$sd_audio (pore )  Ssd_audio (conf_id)  Saudiofmc  $sd_sess (ttl ) ] 

global  vat 

exec  Svat  -C  Ssd_sess (name   Spackecfmt  Sconfaddr  U 
) 

proc  scarc_video  I }  { 

plocal  sd_se3s  sd_video 

sec  videcfTiC  "nv" 

foreach  a    $sd_video (accribuces )  ( 

case  $a  ( 

f mt : *  (  sec  videofmt  [sCring  range  $a  4  end]  ) 

} 
) 
case  Svideofmc  i 

nv  ■' 


} 
ivs  { 


jpg  ( 


global  nv 

exec  Snv  -cci  Ssd_sess ( tt 1 )  $sd_sess (address)  \ 
Ssd_video (port )  & 


global  ivs 

exec  Sivs  -a  -r  -T  $sd_sess (CCl )  \ 
desc  $sd_sess (address)  & 


global  imm 

exec  $imm  -p  $sd_video (port )  -I  $sd_sess (address)  \ 
-CCl  $sd_sess ( ccl )  -n  $sd_sess (name)  & 


proc  scart_whiteboard  ( )  { 

global  sd_sess  sd_whiteboard  wb 
set  orient  "-1" 

foreach  a  $sd_whi teboard (attributes)  { 
case  $a  { 

orient :portrait  {  set  orient  -p  ) 
orient : landscape  {  set  orient  -1  } 
orient : seascape  (  set  orient  +1  } 
orient :dis-npsnec  { 
global  nps 

exec  Snps  -F  "config.trg"  -p  $sd_video (port )  \ 
-I  $sd_sess (address)  -t  $sd_sess ( ttl) 
& 

#  do  not  also  execute  whiteboard 
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Ssd_whi!:eboard  (port )    \ 


} 


return 

} 

orient : dis-auv-uvw  { 

global  dis_auv_uvw 

global  dis_auv_uvv;_dir 

cd  $dis_auv_uvw_dir 

exec  Sdis_auv_uw;  -port 


-address  $sd_sess (address)  & 
cd 
#  do  not  also  execute  whiteboard 

return 


global  xpsview 

i  running  xpsview  is  a  bug  fix  to  prevent  irix  5.2  crash 
puts  stdout   "  " 

puts  stdout   "Spawning  dummy  xpsview  to  avoid  whiteboard  bug  crash  under 
Irix  1.2    ..." 

exec  Sxpsview   a, 

exec  $wr'  -t  $sd_sess ( tt 1 ;  -C  wb : $sd_sess (name)  Sorient  \ 
$sd_sess (address) / Ssd_whiteboard (port )  & 

proc  create_session  ()  { 
) 

proc  neard_session  f )  { 
i 

proc  de-ete_session  { )  { 
} 

»  set  ai-  Hiedia  option  menus  for  new  session  windows. 

set  sd_menu (whiteboard)         "orient:  portrait  landscape  seascapeX 

dis-npsnet  dis-auv-uvw" 

set  sd_menu  (audio)      "fmt:  pcm  pcm.2  pcm4  idvi  dvi2  dvi4  gsm  lpc4  nvt" 

sec  pd_menu (video  I      "fmt:  nv  ivs  imm" 

*»  set  ui:  the  command  names 

*  Ed:t  these  to  match  your  system  locations  &  filenamies,  if  necessary. 
«  The  version  of  mosaic  that  you  point  to  must  not  have  a  precompiled 
*t    local  home  page  hard-wired  in  the  binary,  or  it  won't  redirect. 

set  vat  " /n/elsie/work3/macedoni/video/vat " 

set  nv  " /n/elsie/work3 /macedoni/video/nv" 

3et  ivs  " /n/elsie/work3/macedoni/video/ivs" 

set  wb  " /n/elsie/work3/macedoni/video/wb_src/wb" 

set  imm  " /n/elsie/work3/macedoni/video/imni" 

set  mosaic  • /n/dude/work/brutzman/xmosaic/Mosaic-sgi " 

set  xpsview  xpsview 

set  npsnet  " /n/bossie/work3/npsnetIV/npsnetIV" 

set  dis_auv_uvw_dir     " /n/dude/work/brutzman/auv-uvw" 
set  dis  auv  uvw  viewer 
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F.  .mailcap  Configuration  File  for  Mosaic  Multimedia  Viewers 

##!(##*■##*#«««#«#####«######################################################### 

#  qZ-.bal  mailcap  file  for  mosaic  on  NPS  gra-vry  net    Don  Brutzman    18  OCT  94  « 

«t  glotal  unix  systemi  location:  /usr/local/lib/mosaic/mailcap      # 

#  users  can  put  their  own  preferences  in  -yourname/ .mailcap  # 

#  # 
4»  edit  directory  paths  as  necessary'  for  your  machine  # 

#  # 

#  reference:   http://www.ncsa.uiuc.edu/SDG/Software/Mosaic/Docs/mailcap.html  # 

#  # 

#  URL  of  this  example  file  is:  # 

#  ftp://taurus.cs.nps.navy.mi1/pub/auv/.mailcap.auv-uvw  # 

#  # 

application/postscript;  xpsview  %s 

#  If  you  wish  to  use  xplaygizmo  (  ftp://ftp.ncsa.uiuc.edu/Mosaic/misc/  ) 

#  video/mpeg;  xplaygizmo  -p  mpeg_play  %s 

#  .Alternate  mpeg_player  which  includes  looping  capability: 
videC;  mpec;  /n/dude/work/brut  zman/xmosaic/mipeg_player  %s 

#  I  don't  tn:nk  xplaygizmo  is  needed  for  small  audio,  but  it  is  workable. 

#  It  IS  helpful  on  longer  files  such  as  Internet  Talk  Radio. 

a.udio/*;  /n/'dude/work/brutzman/xmosaic/xplaygizmo  -p  -q  sfplay  %s 

«     aovj^w         ;      SiP'idy     ^P 

#  STILL  NEEDED:  dvi  viewer  (such  as  the  one  which  works  for  mosaic  on  grus) 
appiication/x-dvi ;  xdvi  %s 

# 

#  Note:   besides  Mosaic,  mailcap  files  are  used  by  MIME-capable 

#  miultimedia  mail  handlers 

#  Final  note:   be  sure  to  select  /Options  _Reload_Conf ig  Files  from  the  menu 

#  for  any  changes  to  be  effective  in  your  current  session. 
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G. 


.cshrc  Excerpts:    Login  Configurations  for  MBone  and  Mosaic 


#  mbone  aliases/paths  (from  -brutzman / . C9hrc] 

#  5d 


CO  stare  the  session  directory  tool,  everything  else 
IS  invoked  from  the  sd  menu 


set  path  =  (Spath  -macedoni /video        ~macedoni/bin) 

set  path  =  (Spath  -macedoni /video/wb_src  ~macedoni/video/mm_src ; 


a.:  i!:r  3a 
alias  vat 
alias  nv 
alia^  wb 
alias  XV 
alias  imm 
alias  imiT.2xv 


^  1  ^■a; 


iVS 


-macedoni /video /sd  &  ' 
~macedoni/video/vat ' 
-macedoni  /video/nV 
-macedoni /video /wb_src/wb' 
-macedoni /video/xV 
-macedoni /video /imm' 
-macedoni /video/mm2xv ' 
-macedoni /video/ivs ' 


#  session  directory 

#  visual  audio  tool 

#  net  video 

#  white  board 

#  xview 

#  image  monitor  tool 

#  image  monitor  =>  xview 

#  INRIA  videoconferencing  system 


ncrmaliy  imm  goes  full  screen,  imm  can  use  imm2xv  for  a  window 


«  al3c  copy  the  file  -brutzman/ . sd. tcl  to  your  home  account 

#  so  that  the  tools  are  initially  set  up  properly! 

#  subscribe  to  local  NPS  list  npsmbone@nps . navy  .mil  if  you  are  interested 

#  mosaic/wcrld-wide-web  browser  aliases/paths  -  for  gravynet  users 

set  path  =  (Spath  -brutzrrian/xmosaic ; 

alias  mosaic  ' -brut zman/xmosaic/Mosaic-sgi 

ftp://taurus.cs. nps . navy .mi  1 /pub/mosaic /nps_mosaic . html#TOC  & ' 

alias  www     ' -brutzman/xmosaic/www 

f tp ; / /taurus . cs .nps . navy .mil /pub/mosaic/nps_mosaic . html  ' 

alias  lynx    ' -brut zman/xmosaic/lynx 

f tp :/ /taurus . cs .nps . navy .mil /pub/mosaic/nps_mosaic .html  ' 

#  also  cc-Y-y    tn*-  .mailcap  file  to  initialize  mosaic 

#  cp  -brutzniari/  .riiailcap  .mailcap 

#  URL  of  this  example  file  is: 

#  f tp : //taurus . cs .nps .navy .mil/pub/auv/ .cshrc -excerpt .auv-uvw 
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